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1  INTRODUCTION 

Research  under  this  contract  began  as  scheduled  on  October  1,  1981.  During 
the  first  eighteen  months,  no  significant  deviations  occurred  from  the  plan 
contained  in  our  proposal  dated  May  13,  1981.  Current  project  milestones  are 
summarized  in  a  list  dated  October  19,  1982,  and  attached  to  our  Quarterly 
R  5  D  Status  Report  for  the  period  October  1,  1982  through  December  31,  1982. 

All  milestones  on  this  list  pertaining  to  the  period  covered  by  the  present 
report  have  been  met  on  schedule.  The  following  pages  present  some  details 
concerning  work  relating  to  each  of  these  milestones.  Further  information  is 
provided  in  the  attached  appendices  and  in  theses,  dissertations,  published 
papers,  and  videotapes  previously  furnished  to  DARPA. 

2.  MECHANICAL  SYSTEM  RESEARCH 

2. 1  Breadboard  Leg  Tests 

The  breadboard  leg  system  was  extensively  tested  using  a  valve-controlled 
hydraulic  actuation  system.  A  bypass  control  valve  was  used  on  the  drive 
actuator  with  conventional,  four-way  throttling  valves  on  the  leg  lift  and 
abduction-adduction  actuators.  The  drive  actuator  was  a  balanced  area  actuator, 
while  the  other  actuators  were  single-sided.  All  were  linear  actuators. 

They  were  supplied  by  variable  displacement  pumps  operating  in  a  pressure 
regulated  mode.  Figure  1  shows  the  breadboard  leg  mounted  in  its  test  stand. 

The  system  was  successfully  operated  using  both  analog  and  digital 
controllers.  The  latter  was  used  during  data  collection  runs.  The  digital 
stride  cycle  control  program  was  successfully  modified  to  minimize  shock 
associated  with  changes  in  leg  direction  and  to  optimize  dynamic  response. 

The  system  was  tested  at  equivalent  ground  speeds  in  excess  of  3  mph.  The  leg 
was  also  static  loaded  in  excess  of  its  design  load  of  2000  lb.  Details  of 
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the  test  program  and  results  may  be  found  in  the  thesis  "Modeling  and  Control 
of  the  Hydraulic  Drive  System  for  a  Walking  Machine  Leg"  by  John  F.  Gardner. 

A  series  of  energy  consumption  tests  were  also  run  on  the  leg.  The  leg 
exhibited  a  specific  resistance  of  0.078.  This  figure  includes  actuator  losses, 
but  not  pump  or  drive-train  losses.  Allowing  for  these,  and  for  the  overhead 
power  needed  to  run  the  system  electronics,  a  specific  resistance  comparable  to 
that  of  tracked  vehicles  appears  readily  attainable. 

Static  and  dynamic  deflection  studies  have  also  been  performed  on  the  leg 
structure  and  compared  to  the  results  of  finite  element  models.  Structural 
stiffness  is  adequate. 

2 . 2  Hydraulic  Circuit  Design  Studies 

Extensive  design  studies  were  performed  on  an  alternative  power  transmission 
and  actuation  system  concept  using  hydrostatic  transmissions.  Comparative  cost, 
weight,  and  performance  studies  were  conducted  for  the  hydrostatic  system  and 
the  best  valve-controlled  configuration.  The  hydrostatic  system  has  lower  cost, 
lighter  weight,  and  less  power  demand.  Its  dynamic  response  is  not  markedly 
inferior,  and  it  has  the  advantage  of  much  greater  simplicity  and  modularity 
in  both  hardware  and  in  control.  Therefore,  it  was  decided  to  proceed  to 
detailed  design  of  a  fully  hydrostatic  system.  It  is  also  planned  to 
reconfigure  the  actuation  system  of  the  breadboard  leg  as  a  hydrostatic  system 
for  a  further  series  of  tests.  Dynamic  simulation  and  bench-test  studies  of 
series-bypass  valve-controlled  drive  circuits  are  continuing.  It  is  intended 
to  augment  them  with  dynamic  simulation  studies  of  the  hydrostatic  configuration. 

2 . 3  Frame  Tests 

The  vehicle  frame  was  constructed.  It  weighs  315  lb.  It  was  suspended 
from  soft  springs  to  approximate  a  free-free  condition  and  dynamic  vibration 
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mode  tests  were  performed  using  shaker  excitation.  The  results  showed 
reasonable  agreement  with  those  of  finite  element  models  used  in  the  design. 

The  frame  is  scheduled  for  static  deflection  tests  during  the  next  reporting 
period.  Figure  2  shows  the  completed  frame. 

2.4  Leg  Design 

The  design  of  the  legs  to  be  used  on  the  vehicle  is  well  advanced. 

Special  actuators  for  the  drive  and  leg  lift  have  been  designed.  Paired 
actuators  mounted  on  either  side  of  the  leg  box  will  be  used  in  both  cases 
to  prevent  twisting  moments  on  the  roller  assemblies.  In  order  to  save 
space,  the  equal  area  actuators  will  be  mounted  in  both  cases  with  the  rods 
anchored  to  the  leg  box  and  the  bodies  of  the  cylinders  coupled  to  the  slides. 
Fluid  will  be  fed  through  passages  in  the  piston  rods.  This  arrangement 

eliminates  the  need  for  flexible  hydraulic  connections  and  minimizes  the  active 
fluid  volume. 

It  was  decided  to  use  a  hinged  ankle  rather  than  the  spherical  joint  used 
on  the  breadboard  leg  and  to  add  a  mechanism  to  maintain  the  foot  in  an 
attitude  parallel  to  the  vehicle  body  at 'all  times.  A  roller  chain  mechanism 
has  been  designed  for  this  purpose. 

The  new  leg  structure  is  fairly  similar  to  that  of  the  breadboard  leg. 

The  geometric  proportions  of  the  upper  and  lower  links  have  have  altered  in 
accordance  with  the  improved  geometry  detailed  in  Figure  3.  This  results  in 
a  larger,  more  symmetric  working  envelope.  Figure  4  shows  the  revised  leg 
mounting  position  and  overall  dimensions. 

2 . 5  Other  Work 

A  model  of  the  current  vehicle  configuration  concept  was  completed  and 
is  shown  in  Figure  5.  It  is  one-tenth  scale  in  size.  The  legs  are  movable 
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Figure  2:  Completed  frame  mounted  for  free  vibration  mode  tests. 


Figure  4:  Dimensions  and  leg  mounting  positions  for  revised  vehicle  geometry. 
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in  all  three  degrees  of  freedom  with  friction  drag  to  hold  position.  Table 
1  lists  the  design  goals  for  this  vehicle  concept. 

Simulation  studies  of  extreme  obstacle  crossing  maneuvers  were  completed 
using  the  Evans  and  Sutherland  PS  300  display  system.  Leg  movements  were 
determined  by  the  programmer  using  trial  and  error.  Thus,  this  can  be  regarded 
as  a  simulation  of  the  precision  footing  mode.  Crossings  of  a  five  foot, 
vertical-wal led  ditch  and  of  a  five  foot  vertical  step  in  both  directions  were 
simulated.  A  frame  from  the  display  is  shown  in  Figure  6. 

3.  ELECTRICAL  SYSTEM  RESEARCH 

3. 1  Breadboard  Computer 

Construction  of  a  breadboard  version  of  the  ASV-84  control  computer 
has  been  completed.  This  computer  consists  of  nine  Intel  86/30  single¬ 
board  computer  systems.  Each  board  contains  an  8086  processor,  an  8087 
co-processor,  16  channels  of  A  to  D  conversion,  and  8  channels  of  D  to  A 
conversion.  Six  of  the  computers  are  assigned  to  leg  control;  i.e.,  one 
for  each  leg.  One  computer  is  used  for  implementation  of  cockpit  functions, 
and  another  is  used  as  a  coordination  computer.  Finally  the  remaining 
computer  implements  safety  and  diagnostics  functions  in  addition  to  serving 
as  a  co-processor  for  the  coordination  computer. 

The  current  hardware  design  for  the  Battelle  guidance  system  makes  use 

of  four  additional  86/30  boards,  raising  the  total  for  the  ASV-84  system  to 

thirteen.  The  coordination  computer  is  connected  to  the  guidance  computers 

by  means  of  a  parallel  line  unit  and  to  all  other  computers  by  an  Intel 

multibus.  All  multibus  communications  are  initiated  by  the  coordination 
computer,  thereby  eliminating  the  possibility  of  bus  access  conflicts. 
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Table  1 


Design  Goals  for  1984  Proof-of-Concept  Vehicle  (ASV-84) 


Level  walking  cruise  speed  of  5  mph. 


Range  of  50  miles  without  refueling. 


Payload  of  500  lb. 


Vehicle  dry-weight  around  4000  lb. 

Capable  of  climbing  60  percent  slope  on  suitable  soil. 

Length  about  14  ft.,  nominal  height  to  top  of  frame  around 
7  ft.,  10  ft.  to  top  of  terrain  scanner. 

Cross  7  ft.  ditch  with  vertical  walls,  climb  5  ft.  vertical 
step,  traverse  4  ft.  isolated  vertical  object. 


Belly  skids  for  parking  and  safety. 


Portions  of  the  breadboard  computer  have  been  used  to  control  the 
electric  monopod  developed  in  the  first  year  of  this  project.  Anticipated 
cycle  times  were  achieved,  increasing  our  confidence  in  the  suitability  of 
the  86/30  board  for  hydraulic  leg  control.  Details  of  this  experiment  can 
be  found  in  the  thesis  of  C.E.  Barrientos,  listed  in  Appendix  1. 

The  next  stage  of  testing,  presently  scheduled  for  completion  in 
September,  1983,  will  demonstrate  that  the  breadboard  computer  possesses 
sufficient  computing  power  to  control  the  ASV-84  in  real-time  in  the  terrain 
following  mode.  These  experiments  will  utilize  the  PDP-11/70  computer  to 
simulate  the  vehicle,  scanner,  and  terrain. 

3. 2  Binocular  Vision  System 

The  OSU  Hexapod  vehicle  has  been  fitted  with  two  CID  television  cameras 
to  permit  laboratory  experiments  relating  to  the  integration  of  optically- 
sensed  range  information  with  ground  reaction  force  and  body  attitude 
measurements.  The  two  cameras  derive  range  data  by  triangulation  on  candidate 
footholds  designated  by  a  human  operator  using  a  hand-held  He-Ne  laser.  The 
system  software  then  accepts  or  rejects  such  footholds  based  on  the  kinematic 
capabilities  of  the  machine.  Accepted  footholds  are  used  by  succeeding  legs 
by  means  of  a  "follow-the-leader”  gait.  Details  are  provided  in  the  attached 
Appendix  3. 

The  primary  significance  of  this  experiment  is  that  it  demonstrates  in  a 
physical  experiment  the  feasibility  of  the  methods  we  propose  for  "fusing"  the 
output  of  the  Battelle  guidance  computer  together  with  other  sensory  information 
to  smoothly  guide  the  ASV-84  machine  over  rough  terrain.  A  videotape  documenting 
this  capability  was  provided  to  DARPA  in  December,  1982.  A  secondary  importance 
of  the  results  obtained  is  that  follow-the-leader  gaits  seem  to  be  well  suited 
to  climbing  over  large  obstacles  in  a  "precision  footing"  mode  of  control. 
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3.3  Safety  and  Diagnostics  Software 


A  set  of  definitions  and  general  characteristics  of  the  ASV-84  safety 
and  diagnostics  software  has  been  developed.  This  information  was  presented 
at  a  design  review  held  in  Naples,  Florida  on  March  8-9,  and  subsequently 
adopted  as  a  guide  for  the  development  of  more  detailed  software  specifications 
and  vehicle  operating  procedures.  This  material  is  included  in  this  report 
as  Appendix  2. 

3.4  Terrain-Following  Software  Test 

The  compatibility  of  Battelle  guidance  software  and  OSU  autopilot 
software  for  the  ASV-84  has  been  verified  by  a  simulation  test  on  the 
PDP-11/70  computer.  This  test  included  climbing  slopes  and  stepping  over 
ditches  automatically.  The  response  of  the  vehicle  was  presented  in 
real-time  on  a  vector  graphics  unit.  For  more  detailed  understanding,  any 
selected  frame  of  the  vector  graphics  display  can  be  converted  to  shaded 
color  graphics.  Figure  7  shows  one  frame  from  the  latter  type  of  display. 

Work  in  progress  will  permit  the  extension  of  this  capability  to  the  production 
of  motion  pictures  of  shaded  color  graphics  representations  of  vehicle  motion. 

The  simulation  tests  revealed  only  minor  incompatibilities  between  OSU 
and  Battelle  software  and  these  were  all  corrected.  The  next  stage  of  testing, 
presently  scheduled  for  September,  1983,  will  include  demonstration  of  hardware 
compatibility  between  the  OSU  and  Battelle  components  of  the  breadboard  computer. 

3.5  Proximity  Sensors 

An  ultrasonic  proximity  sensing  system  suitable  for  laboratory  use  with 
the  OSU  Hexapod  has  been  designed  and  tested.  Results  are  fully  satisfactory 
so  long  as  the  incidence  angle  between  the  terrain  and  sensor  is  restricted 
to  less  than  abou  30°  from  vertical.  When  this  angle  is  exceeded,  insufficient 
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energy  is  reflected  back  to  the  receiver,  and  distance  information  is  lost. 

It  is  felt  that  rough-terrain  locomotion  in  the  close  maneuvering  mode 
can  be  accomplished  in  the  laboratory  using  the  present  sensor  design. 

This  will  be  accomplished  by  choosing  leg  trajectories  such  that  incidence 
angle  constraints  are  satisfied.  Additional  sensing  systems  are  now  being 
constructed  and  installed  so  that  each  leg  of  the  OSU  Hexapod  will  be  able 
to  measure  its  elevation  above  the  terrain.  It  is  expected  that  locomotion 
using  this  type  of  feedback  will  be  demonstrated  by  September,  1983. 

With  respect  to  the  ASV-84,  we  anticipate  that  a  more  advanced  type  of 
proximity  sensor  will  be  needed.  We  intend  to  carry  out  an  evaluation  of 
the  Kodak  multiple  frequency  "chirp"  sensors  in  the  next  six  months  using 
the  breadboard  leg  as  a  test  fixture.  These  tests  will  be  conducted  in  a 
soil  bin  using  various  types  of  terrain  to  more  realistically  simulate  an 
outdoor  environment.  We  will  also  study  the  use  of  sensor  arrays,  both 
ultrasonic  and  optical,  as  a  means  of  solving  the  incidence  angle  problem. 

4.  SUMMARY  AND  CONCLUSIONS 

The  research  supported  by  this  contract  is  on  schedule  and  within 
budget.  Breadboard  leg  tests  have  validated  the  pantograph  leg  geometry  as 
being  appropriate  to  full-scale  vehicle  operation.  While  valve-controlled 
operation  produced  adequate  speed  and  precision  of  motion,  issues  of  system 
weight,  energy  consumption,  and  ease  of  control  now  favor  a  hydrostatic, 
pump-controlled,  hydraulic  actuation  system. 

A  baseline  software  system  for  the  ASV-84  coordination  computer  has  been 
successfully  interfaced  and  tested  with  Battelle  guidance  software.  The 
breadboard  computer  has  been  completed.  Fusion  of  optically  sensed  range 
information  with  other  sensory  data  for  vehicle  control  has  been  demonstrated 


using  the  OSU  Hexapod  as  a  test  bed.  Providing  subsystems  from  other  contractors 
are  delivered  on  time,  there  is  at  present  no  reason  to  doubt  that  a  successful 
outdoor  demonstration  of  terrain- following  locomotion  will  be  achieved  as 
planned  during  August,  1984. 
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Appendix 

Safety  and  Diagnostics  Softv 


SAFETY  AND  DIAGNOSTICS  SYSTEM  DEFINITIONS 


SAFETY:  Failure  detection  during  vehicle  operation  followed  by 
transition  to  a  safe  shut-down. 

DIAGNOSTICS:  Isolation  of  a  failure  to  a  replaceable  module. 


Safety  functions  apply  only  when  hydraulic  power  is  applied  to 
legs;  i.e.,  safety  software  is  real-time  and  on-line, 

Diagnostic  procedures  are  off-line  and  interactive,  but  automated 
to  the  greatest  extent  possible.  Generally  applied  only  with 
vehicle  in  a  safe  halt  state. 


SHUT-DOWN  MANEUVERS 


CONTROLLED  STOP:  Vehicle  comes  to  a  gradual  halt  under 
computer  control  following  detection  of  a  non-catastrophic 
failure;  e.g.,  loss  of  prime  mover  power  (halt  accomplished 
with  energy  stored  in  flywheel) . 

MOTION  FREEZE:  All  actuators  locked  by  closing  servo 
valves  following  suspected  leg  control  failure.  Might  be 
achieved  by  interrupting  primary  power  to  all  valve  drivers. 

HARD  LANDING:  Drops  machine  onto  belly  skids  in  minimum 
time  following  malfunction  of  cockpit  or  guidance  computer, 
vertical  reference  failure,  incipient  instability,  operator 
panic,  etc.  Probably  requires  functioning  coordination 
computer  and  leg  servos. 
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FAILURE  CATEGORIES 


SOFTWARE:  Algorithms  cannot  be  completely  evaluated 
prior  to  field  testing.  Unexpected  loads,  operator 
error,  new  terrain  conditions,  etc.,  can  lead  to 
unsafe  motion  planning  or  motion  execution. 

HARDWARE:  Mechanical  component  failures  can  generally 
be  detected  by  cross-checking  of  transducers  and  by 
out  of  range  error  signals.  Not  all  computer 
failures  can  be  directly  detected. 

PRIMARY  SYSTEMS:  Safety  computer  can  be  directly 
connected  via  A  to  D  converters  to  primary  system 
transducers  such  as  engine  oil  pressure,  D.C.  supply 
voltage,  hydraulic  pressure,  vertical  gyro,  etc. 


GENERAL  APPROACH  TO  FAILURE  DETECTION 


Safety  function  is  distributed  to  minimize  bus  traffic. 

Detected  errors  set  status  bits  in  global  memory  of 
affected  computer.  No  interrupts  to  higher  level. 

Each  computer  checks  its  own  peripherals.  (A  peripheral 
can  be  a  transducer,  a  leg,  or  the  entire  body  depending 
on  position  of  the  computer  in  the  control  hierarchy.) 

Data  from  peripherals  stored  in  circulating  buffers  to 
permit  detection  of  inconsistencies  and  subsequent  diagnosis 

Critical  sensors  could  be  duplicated;  e.g.,  two  vertical 
gyros. 

Cross-checks  possible  on  related  sensors;  e.g.,  integrate 
tachometers  and  compare  to  position  transducers. 

Safety  computer  is  a  co-processor  to  coordination  computer; 
accesses  bus  directly  only  if  coordination  computer  failure 
suspected. 


DIAGNOSTIC  SEQUENCE 


1.  Put  vehicle  in  safe  shutdown  configuration. 

2.  If  error  detected  at  coordination  level,  perform  computer 
tests  first.  Then  go  to  transducer  and  I/O  check. 

3.  If  error  detected  at  leg  level,  do  transducer  and  I/O  test. 

4.  If  automatic  routines  fail,  provide  prompts  to  maintenance 
personnel  and  graphical  displays  of  stored  operational 
dat3. 
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DIAGNOSTIC  PROCEDURES 


Safety  computer  functions  as  a  circulating 
duplex  computer  checking  each  other  computer 
in  turn  via  multibus  or  other  connections. 

Transducer  function  tested  by  cross-checking 
of  related  sensors;  e.g.,  position  and  force 
are  related. 

Transducer  failure  cannot  be  automatically 
distinguished  from  A  to  D  or  I/O  failure. 
Requires  human  intervention  with  auxiliary 
test  instruments  and/or  direct  human  sensing 
(visual,  tactile,  etc.). 
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Chapter  1 
INTRODUCTION 


1.1  Historical  Background  of  Legged  Vehicles 

It  has  been  observed  for  some  time  that  biological  systems  have 
better  mobility  and  adaptability  on  rough  terrain  than  conventional 
wheeled  vehicles.  People  became  interested  in  walking  machines  for 
their  potential  applications  in  circumstances  which  are  not  suitable  for 
wheeled  vehicles  and  environments  that  are  considered  hostile  to  a  human 
being.  Examples  are  remote  planet  exploration,  fire  fighting,  material 
nandling  in  nuclear  power  plants,  and  mining  [1]. 

Recent  advances  in  the  fields  of  computer  technology,  automatic 
control,  and  a  better  understanding  of  animal  locomotion  from  a 
theoretical  point  of  view  have  made  the  realization  of  legged  walking 
robots  possible.  Several  machines  were  constructed  during  the  past  two 
decades.  Some  of  tnese  are  mentioned  below.  Each  of  these  machines  has 
its  own  characteristics  and  represents  a  distinct  stage  of  the  evolution 
of  walking  vehicles. 

The  Space  General  Linkage  Walker  [2]  was  an  eight-legged  machine 
developed  in  1960.  It  was  operated  by  pure  mechanical  linkage  inter¬ 
action  without  involving  any  electronic  control  circuitry.  The  drawback 
of  this  vehicle  was  its  limited  adaptability,  owing  to  the  mechanical 
linkage  operation.  However,  it  did  demonstrate  remarkable  off-road 
mobility. 
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The  General  Electric  Quadruped  [3]  was  a  huge  machine  weighing 
about  3000  pounds.  It  was  controlled  by  a  human  operator  in  a  master- 
slave  fashion.  The  machine  followed  or  mimicked  the  motion  of  the 
operator's  hands  and  feet  with  its  front  and  rear  legs  respectively . 

This  was  a  very  cumbersome  task  and  demanded  a  lot  of  skill.  It  was 
concluded  that  a  more  efficient  way  of  controlling  the  machine  was 
necessary. 

The  Phony  Pony  [4]  was  the  first  walking  machine  to  operate  under 
the  control  of  a  special  purpose  digital  computer.  The  machine  was 
built  to  show  that  the  joint  coordination  problem  could  be  solved  by  a 
computer. 

Compared  with  the  Phony  Pony,  the  QSU  Hexapod  [5]  is  a  much  more 
sophisticated  machine  in  terms  of  mechanical  structure,  electronic  hard¬ 
ware,  and  software  algorithms.  It  was  constructed  in  1977,  and  since 
then  a  series  of  experiments  have  been  conducted  to  study  computer  hard¬ 
ware  and  software  organization  for  automatic  limb  motion  coordination. 
Above  all,  a  supervisory  control  structure  [5]  was  successfully  esta¬ 
blished.  This  structure  will  be  examined  in  the  next  section.  Based  on 
experience  with  the  Hexapod,  another  new  vehicle  called  the  OSU  Adaptive 
Suspension  Vehicle  (ASV)  is  now  under  construction  and  is  scheduled  to 
oe  completely  assembled  and  tested  by  1984. 

1.2  The  Hierarchical  Control  Structure  of  the  OSU  Hexapod 

Although  it  is  a  highly  desirable  goal  to  automate  the  OSU  Hexapod 
to  the  greatest  extent  possible,  every  step  toward  the  realization  of  a 
fully  autonomous  machine  must  be  made  with  consideration  for  current 
technology  and  for  available  resources.  It  is  believed  that  a 
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Fol low-The-Leader  (FTL)  operation  is  an  important  milestone  toward  that 
goal  and  is  feasible  at  this  time.  In  such  an  operation,  an  operator 
will  control  the  front  feet  by  specifying  footholds  for  them  and  the 
other  legs  will  follow  their  footprints  as  the  vehicle  moves  forward, 
thus  only  footholds  which  are  assigned  by  the  operator  will  be  used. 

The  relationship  between  a  FTL  operation  and  a  completely  automatic 
vehicle  can  be  best  illustrated  by  referring  to  the  control  structure  in 
Fig.  1.1. 

This  structure  is  called  a  supervisory  control  scheme  because  a 
human  operator  is  involved  in  providing  high  level  commands.  The 
function  of  the  control  block  between  the  operator  and  the  vehicle  is  to 
convert  the  commands  from  the  former  into  the  physical  movements  of  the 
latter.  Depending  on  how  the  control  block  is  implemented,  the  control 
hierarchy  is  divided  into  four  levels. 

The  lowest  level  is  called  the  master-slave  mode.  In  this  mode, 
the  control  block  virtually  does  not  exist  since  the  operator  has  to 
manually  maneuver  the  machine  in  all  aspects.  The  operation  of  the  GE 
Quadruped  is  of  this  nature. 

The  next  higher  level  is  the  joystick  control  mode.  Here  the  oper¬ 
ator  gives  such  commands  as  steering  anr1  speed  through  a  joystick  in  a 
similar  way  to  driving  an  automobile,  and  a  digital  computer  furnishes 
all  the  low  level  signals  needed  to  cause  the  Hexapod  to  generate  the 
desired  behavior  [5]. 

To  carry  it  one  step  further,  in  the  FTL  mode,  the  operator  even 
does  not  have  to  take  the  wheel.  He/she  simply  indicates  a  foothold  and 
lets  the  computer  figure  out  the  detailed  subcommands  to  direct  the 
vehicle  toward  the  destination. 


Finally,  after  sufficient  intelligence  is  built  into  the  machine  so 
that  it  is  able  to  sense  and  analyze  the  environment,  and  to  select  a 
proper  route  as  it  moves  along,  the  role  of  the  operator  is  no  longer 
needed  and  can  be  eliminated.  As  such,  the  machine  becomes  self- 
contained  and  fully  automatic. 

The  objective  of  this  dissertation  is  to  study  and  implement  the 
FTL  operation.  Toward  this  end,  a  binocular  vision  system  based  on  two 
solid-state  TV  cameras  has  been  designed  and  constructed  to  sense  the 
terrain.  In  addition,  the  control  of  the  stepping  motions  of  the 
Hexapod  is  conceptualized  and  a  new  method  of  coordinating  stepping  of 
successive  legs  is  presented  in  which  only  footholds  which  have  been 
tested  by  preceding  legs  in  rough-terrain  locomotion  are  used.  Finally, 
all  the  hardware  and  software  are  tested  and  integrated  together  to 
ensure  a  reliable  operational  system. 

1.3  Organization  of  the  Dissertation 

Chapter  2  is  a  review  of  related  work,  including  the  constituting 
elements  of  a  vision  system,  industrial  robotics  systems  that  utilize  a 
vision  function  to  aid  their  operation,  and  existing  mobile  vehicles 
which  possess  vision  capability.  Available  schemes  for  range  measure¬ 
ments  are  also  discussed.  Previous  research  done  on  the  Hexapod  is  also 
covered. 

In  Chapter  3,  the  binocular  vision  system  is  presented.  All 
components  needed  to  facilitate  FTL  operation  are  described.  Design 
and  implementation  of  the  hardware  interface  circuit  is  explained  in 
detail . 
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Chapter  4  describes  calibration  procedures  for  the  constructed 
vision  system.  This  includes  camera  calibration  and  lens  calibration  as 
well  as  the  identification  of  the  geometric  relationship  between  cameras 
and  the  Hexapod.  Coordinate  reconstruction  based  on  triangul ation  is 
also  discussed. 

Chapter  5  presents  algorithms  that  direct  the  Hexapod  to  generate 
FTL  motion,  with  an  emphasis  on  the  activities  occurring  at  the 
locomotion-cycle  level. 

In  Chapter  b,  modifications  that  are  needed  to  enable  the  Hexapod 
to  walk  over  rough  terrain  using  algorithms  developed  in  Chapter  5  are 
di scussed. 

The  vision  systan  is  evaluated  in  Chapter  7.  Experimental  results 
with  the  Hexapod  walking  on  even/rough  terrain  are  also  shown. 

Finally,  in  Chapter  8,  research  contributions  of  this  work  are 
summarized  and  possible  extensions  are  recommended. 

Programs  related  to  the  vision  system  and  walking  algorithms  are 
listed  in  an  appendix. 
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CHAPTER  2 


REVIEW  OF  PREVIOUS  WORK 


2.1  Introduction 

In  recent  years,  there  has  been  a  trend  to  develop  advanced  auto¬ 
mation  techniques  to  increase  industrial  productivity  and  to  enhance  the 
well-being  of  workers  by  eliminating  hard,  dangerous  and  dull  jobs 
[6,7].  Along  this  direction,  robotics  has  attracted  a  great  deal  of 
attention  and  induces  intensive  research  efforts  [8].  To  perform  cer¬ 
tain  hunan-like  tasks,  a  robot  must  be  able  to  sense  its  internal  states 
and  its  envi ronment.  Use  of  different  types  of  sensors,  such  as  visual 
sensors,  force  sensors,  acoustic  sensors,  and  temperature  sensors  have 
been  investigated  [9].  Among  these,  the  most  important  one  may  be  the 
visual  sensor.  An  old  sayi ng  goes  that  a  picture  is  worth  a  thousand 
words.  While  other  sensors  provide  only  specific  pieces  of  information, 
visual  sensors  have  the  potential  of  being  used  by  a  robot  to  construct 
models  of  the  scene,  and  to  specify  the  positions,  shape,  etc.  of  the 
objects  in  an  appropriate  geometric  representation  [10],  This  is  also 
true  for  biological  systems.  Without  vision,  a  human  has  to  rely  mainly 
on  contact  sensing,  which  is  inefficient  due  to  its  point-by-point, 
trial -and-error  nature,  and  is  sometimes  dangerous. 

One  of  the  central  issues  in  realizing  the  FTL  operation  is  for  the 
Hexapod  to  recognize  and  locate  footholds  specified  by  the  operator. 
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This  task  could  typically  be  well  handled  by  an  optical  sensing  system 
[11,12,13],  specifically,  a  computer  vision  system.  For  this  reason, 
and  in  recognition  of  the  increasing  importance  of  vision  systems  as 
cited  above,  it  was  decided  to  employ  a  vision  system  in  the  hope  that, 
in  addition  to  solving  problems  related  to  FTL  operation,  it  would  be¬ 
come  a  beneficial  tool  for  future  developments  on  the  Hexapod. 

Vision  systems  are  of  various  types  and  are  usually  tailored  to 
suit  particular  applications  [14].  Thus,  it  is  important  to  know  the 
constituting  elements,  the  role  they  play,  and  possible  alternatives  of 
a  vision  system.  This  subject  is  addressed  in  Section  2.2.  To  provide 
a  clearer  picture  of  the  state  of  the  art,  several  existing  systems 
which  utilize  vision  systems  to  aid  robots  in  an  industrial  environment 
are  examined  in  Section  2.3.  Since  the  Hexapod  is  a  mobile  vehicle,  it 
is  of  interest  to  understand  how  a  vision  system  can  improve  the  capa¬ 
bilities  of  a  machine  of  this  class.  Five  examples  are  studied  in 
Section  2.4.  Range  measurenent  is  a  crucial  part  in  implementing  FTL 
operation,  therefore  possible  techniques  to  solve  this  problem  are  dis¬ 
cussed  in  Section  2.5.  At  the  end,  previous  work  that  has  been  done  on 
the  Hexapod  is  briefly  reviewed  in  Section  2.b. 

2.2  Elements  of  a  Computer  Vision  System 

A  typical  computer  vision  system  includes  an  image  sensor,  an  image 
processor  with  its  own  storage  memory,  and  a  set  of  processing  algor¬ 
ithms  stored  in  a  host  computer.  A  display  unit  is  optional,  but  is 
highly  desirable  in  a  man-machine  interacting  environment  where  an  oper¬ 
ator  monitors  the  on-going  process  at  the  display  unit  and  responds 


with  commands  whenever  needed.  Fig.  2.1  shows  a  block  diagram  of  a 
general  vision  systan. 

2.2.1  Image  Sensors 

An  image  sensor,  also  known  as  an  image  digitizer,  converts  a 
natural  scene  into  a  numerical  representation  suitable  for  digital  pro¬ 
cessing.  Four  types  of  widely  used  imaging  devices,  image  dissectors, 
vidicon  cameras,  CCD  (Charge-Coupled-Device)  cameras  and  C I □  (Charge- 
Injection-Device)  cameras  are  described  below. 

2.2.1. 1  Image  Dissectors 

A  simplified  diagram  of  an  image  dissector  is  given  in  Fig.  2.2. 
Physically  a  dissector  comprises  an  evacuated  glass  envelope  which  con¬ 
tains  a  photocathode,  an  anode  with  a  small  aperture,  and  a  signal  col¬ 
lecting  plate  which  is  directly  behind  the  aperture  and  collects  elec¬ 
trons  which  flow  through  the  aperture.  The  operation  of  a  dissector  is 
based  on  the  princip’e  of  photoemission,  whereby  electrons  are  produced 
when  light  falls  on  a  photosensitive  material.  A  light  image  of  the 
scene  is  projected  onto  the  translucent  photocathode  by  a  lens  system. 
Since  the  entire  scene  covers  the  photocathode,  the  number  of  electrons 
emitted  at  any  time  from  an  elemental  area  is  proportional  to  the  illu¬ 
minance  of  that  area.  These  emitted  electrons  are  accelerated  toward 
the  anode  by  the  anode  voltage.  In  effect,  the  light  image  on  the 
photocathode  has  been  transduced  into  a  corresponding  electron  density 
image  which  moves  toward  the  anode  under  the  influence  of  the  acceler¬ 
ating  voltage  and  the  focusing  coils.  With  proper  adjustment  of  the 
current  on  the  focusing  coils,  this  electron  density  image  is 
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Figure  2.1  Block  diagram  of  a  General  Vision  System. 


focused  at  the  plane  of  the  aperture.  Obviously  those  electrons  that 
fall  in  the  area  of  the  aperture  continue  on  to  the  signal  plate  and 
constitute  the  output  current.  Since  the  location  of  the  aperture  is 
fixed,  it  is  necessary  to  properly  control  the  deflection  coils  so  that 
the  entire  image  is  scanned  across  the  aperture. 

2. 2. 1.2  Vidicon  Cameras 

Vidicon  cameras  are  popular  imaging  devices  because  they  are 
relatively  inexpensive  and  widely  available.  A  vidicon  tube  is  operated 
under  the  principle  of  photoconductivity,  i.e.,  when  light  falls  on  a 
piece  of  photosensitive  material,  its  conductivity  or  resistance  varies 
in  proportion  to  the  light  intensity.  Fig.  2.3  shows  a  simplified 
drawing  of  a  vidicon  tube.  Here  the  target  is  coated  with  a  transparent 
conducting  film  which  forms  a  video  signal  electrode.  On  top  of  this 
film  is  deposited  a  thin  photosensitive  layer.  This  layer  is  so  thin 
that  the  transversal  resistance  is  high  enough  to  ensure  sufficient  in¬ 
sulation  between  neighboring  points.  Hence,  it  can  be  considered  as  to 
have  a  large  number  of  tiny  resistive  globules  whose  resi stance  de¬ 
creases  on  illumination.  This  layer  is  scanned  in  a  raster  fashion  by 
a  low  speed  electron  beam  from  the  electron  gun  at  the  rear  of  the  tube. 
The  beam  deposits  electrons  on  the  layer,  thus  reducing  its  surface 
potential.  The  two  surfaces  of  the  target  form  a  capacitor,  and  the 
scanning  action  of  the  beam  produces  a  capacitive  current  at  the  elec¬ 
trode  which  presents  the  video  signal.  The  main  drawbacks  of  vidicon 
cameras  are  retention  and  dark  current,  which  is  the  existing  current 
without  illumination. 
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Figure  2.2  An  Image  Dissector. 


Figure  2.3  A  Vidicon  Tube. 


2. 2. 1.3  Solid-State  Camera 

This  type  of  camera  offers  a  number  of  advantages  over  vidicon 
cameras:  light  weight,  small  size,  low  power  consumption,  low  dark 
current,  high  sensitivity  and  lack  of  persistence.  Two  different  kinds 
of  solid-state  cameras  are  currently  available:  Charge-Coupled-Devices 
(CCD)  and  Charge-Injection-Devices  (CID). 

For  imaging  purpose,  CCDs  can  be  considered  as  an  array  of  closely 
spaced  MGS  capacitors  forming  a  shift  register.  Photons  incident  on  the 
semiconductor  generate  a  series  of  charges  on  the  CCD  array.  Charges 
are  transferred  to  the  output  by  applying  a  series  of  clocking  pulses  to 
a  row  of  electrodes  between  the  source  and  the  drain. 

CIDs  resemble  CCDs  except  that  during  sensing  the  charge  is  con¬ 
fined  to  the  image  site  where  it  was  generated  rather  than  being  trans¬ 
ferred  across  the  row.  The  charges  are  read  using  an  X-Y  addressing 
tecnnique  similar  to  that  used  in  computer  memories,  then  injected  into 
the  substrate,  and  the  resulting  displ acanent  current  is  detected  as  the 
video  signal.  Hence,  with  a  properly  designed  scanning  circuitry,  each 
individual  pixel  of  a  CID  may  be  randomly  accessed,  which  is  not  pos¬ 
sible  for  a  CCD. 

2.2.2  Image  Processors 

Image  data  usually  is  of  large  volume.  Consider  a  camera  which  has 
a  256  by  256  resolution,  8-bit  intensity  level  per  image  and  is  oper¬ 
ating  at  30  frames  per  second.  The  resulting  data  rate  is  almost  2M 
bytes  per  second.  Such  a  large  amount  of  data  would  take  the  host  com¬ 
puter  a  long  time  to  process,  and  thus  has  been  recognized  as  one  of 
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the  major  bottlenecks  for  present  vi  sually-driven  robot  tasks  [15].  The 
The  role  of  a  hardwired  image  processor  is  to  free  the  host  computer 
from  the  low  level,  pixel-to-pixel  operations  so  that  it  can  concentrate 
on  executing  high-level  image  processing  algorithms.  Typical  tasks  per¬ 
formed  by  an  image  processor  are  as  follows. 

Image  buffering:  With  sufficient  memory  space,  several  frames  of 
image  data  can  be  stored  by  the  image  processor.  This  gives  the  host 
computer  more  storage  space.  It  also  allows  the  host  computer  to 
process  an  image  from  the  current  frame  without  losing  information  con¬ 
tained  in  the  upcoming  frames.  For  example,  the  VICOM  digital  image 
processor  manufactured  by  the  VICOM  SYSTEM  INC.  is  capable  of  storing 
up  to  Lo  consecutive  frames  [16]. 

Image  reduction:  One  way  to  speed  up  the  processing  is  to  reduce 
the  volume  of  image  data.  This  can  be  done  by  either  reducing  the 
spatial  resolution,  say,  from  256  x  25 b  into  32  x  32  as  General  Motors 
did  in  the  automation  of  inspection  of  transistor  ignition  assemblies 
[17],  or  by  reducing  the  gray-scale  image  into  a  binary  image  [18]. 

Both  techniques  result  in  degradation  of  image  quality;  some  feature 
points  may  be  missing  in  the  former  method  and  the  latter  produces  an 
unsatisfactory  picture  if  the  original  image  has  a  low-contrast  back¬ 
ground. 

Image  preprocessing;  Another  way  to  make  visual  processing  faster 
is  to  provide  fast  preprocessing  hardware  to  handle  time-consuning  but 
straightforward  tasks.  Examples  are  evaluation  of  image  gradient  and 
connectivity  analysis.  Hitachi  has  developed  a  hardware  mask  matcher 
that  finds  the  best  match  between  a  small  (12  x  12)  subfield  of  the  im¬ 
age  and  a  reference  pattern  in  the  assembly  of  transistor  circuits  [1 9 j . 
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Another  interesting  example  is  the  Selspot  System  manufactured  by  the 
Selcom  Company  [20].  Tnis  system  utilizes  a  two-dimensional  PSD 
{Position  Sensitive  photo-Detector)  to  sense  the  center  of  the  light 
image  and  generates  output  signals  which  are  converted  into  precise 
position  information.  Light  sources  suitable  for  the  Selspot  are  light 
emitting  diodes,  infrared  light  sources,  or  laser  sources.  It  provides 
X  and  Y  outputs  whose  amplitudes  are  directly  related  to  the  coordinate 
of  the  detected  light  source.  In  tne  most  recent  version,  these  outputs 
are  digitized  to  give  12-bit  position  data. 

The  large  quantities  of  image  data  that  need  to  be  processed  make 
parallel  computation  a  highly  desirable  feature  in  computer  vision  sys¬ 
tems.  Several  special  archi tectures  have  been  proposed,  including 
cellular  logic  image  processors,  binary  image  processors,  the  Preston- 
Herron  processor,  the  Delft  image  processor,  parallel  pattern  pro¬ 
cessors,  massively  parallel  processors,  local  image  processors,  adaptive 
array  processors  and  mul ti processor  systems  [21,22].  Recently,  chips 
have  been  developed  to  execute  special  computer  vision  functions  at  a 
high  rate  [23].  It  is  expected  that,  as  the  technologies  of  VLSI  ad¬ 
vance,  developments  will  continue  to  improve  chip  performance,  and  other 
computer  vision  algorithms  will  be  successfully  implemented. 

2.2.3  Image  Processing  Algorithms 

The  ultimate  goal  of  a  vision  system  is  to  understand  the  content 
of  a  scene  and  to  extract  useful  information  from  it.  For  example,  for 
a  robot  to  pick  up  a  workpiece  from  a  conveyor,  it  has  to  know  whether 
the  workpiece  is  in  the  scene,  if  other  objects  also  appear  in  the  pic¬ 
ture,  and  has  to  distinguish  the  workpiece  to  be  handled  from  others. 


etc.  A  lot  of  research  and  algorithm  development  has  been  done  in  the 
area  of  image  processing  and  pattern  recognition  [24].  In  general, 
scene  analysis  is  done  in  a  hierarchical  fashion.  First,  the  whole  pic¬ 
ture  is  decomposed  into  several  subregions  according  to  the  particular 
features  (edge,  hole,  corner)  that  have  been  detected  in  each  area. 

Then  relationships  are  established  among  different  areas  by  some  pro¬ 
perties  (shape,  size,  color,  texture)  and  the  associated  regions  are 
labeled  with  attributes  or  descriptions.  Finally,  the  recognition  of  an 
object  is  achieved  by  comparing  its  attributes  with  models  stored  in  a 
library.  The  foregoing  procedures  were  summarized  by  Rozenfeld  [15] 
into  the  four  stages  of  computer  vision  processes:  (1)  restoration,  (2) 
segmentation,  (3)  description,  and  (4)  model  matching.  A  detailed  sur¬ 
vey  of  algorithms  applicable  to  each  stage  is  given  in  [24]. 

2.3  Vision  Systems  Applied  to  Robotics 

Four  existing  systems  are  described  below  to  exemplify  the  role  of 
vision  systems  as  applied  in  industrial  environments  to  enhance  the 
capability  of  a  robot. 

2.3.1  The  General  Motors  CONSIGHT  System 

The  CONSIGHT  system  is  designed  to  transfer  parts  from  a  moving 
conveyor  belt  to  a  predetermined  location  [25].  The  conveyor  carries 
randomly  positioned  parts  past  a  vision  station  which  determines  each 
part's  position  and  orientation  on  the  moving  belt.  The  vision  system 
employes  a  linear  array  camera  (Reticon  RL25oC  256  x  1)  which  images  a 
narrow  strip  accross  the  belt  perpendicular  to  the  belt's  direction  of 
motion.  A  structured  light  source,  a  narrow  and  intense  line  of  light. 


16 


is  projected  across  the  belt  surface  (Fig.  2.4).  When  an  object  passes 
into  the  beam,  the  light  is  intercepted  before  it  reaches  the  belt  sur¬ 
face.  From  the  camera's  view,  there  is  a  dark  area  on  the  belt  where 
the  part  is  positioned  (Fig.  2.5).  The  camera  data  is  accumulated,  as 
the  part  moves  along  the  belt,  so  that  a  two-dimensional  picture  is 
obtained  by  combining  successive  arrays  of  image.  From  this  picture, 
properties  of  the  passing  part  such  as  area,  center  of  area,  X  and  Y 
spans,  number  of  holes,  and  various  moment  invariants  are  computed.  The 
part's  position  is  described  by  the  triple  (X,Y,A),  where  the  X  and  Y 
values  are  selected  as  the  center  of  area  of  the  part  silhouette  and  A 
is  the  axis  of  the  least  moment  of  inertia  of  the  part  silhouette.  The 
robot  is  a  Stanford  arm  made  by  Vicarm.  The  host  computer  is  a  POP- 
11/45  computer.  Since  the  distance  between  the  camera  and  the  belt  is 
fixed  and  known,  the  image  analysis  involved  is  a  two-dimensional 
problem  instead  of  a  three-dimensional  one. 

2.3.2  The  UNIVISION  System 

This  system  is  composed  of  Unimation's  PUMA  250/600  robots  and 
Machine  Intelligence  Corporation’ s  VS-100  vision  system,  and  is  con¬ 
trolled  by  the  VAL  language  [26].  The  organization  of  this  system  is 
given  in  Fig.  2.6.  The  vision  system  initially  thresholds  a  gray-level 
image  into  a  binary  image  and  thereafter  works  with  object  silhouettes. 
It  characterizes  blobs  on  the  basis  of  such  features  as  area,  perimeter, 
minimum  and  maximum  radii,  and  number  of  holes.  The  vision  system  can 
be  "trained"  to  recognize  objects  by  "showing"  sample  objects  to  it. 

It  allows  direct  connection  to  GE  TN250Q  and  Reticon  LC6Q0C  256  x  1 
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Camera 


Figure  2.6  The  UNIVISION  System. 
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cameras.  The  locations  of  visible  objects  as  determined  by  the  vision 
system  are  sent  to  the  robot  for  manipulation. 

2.3.3  The  Westinghouse  Visual  Inspection  and  Industrial  Robot  Control 
System 

This  systan  consists  of  a  G£  TN2200  (128  x  128)  camera,  a  special- 
purpose  preprocessor  which  performs  analog-to-digi tal  image  conversion 
and  data  storage,  a  PDP-11/03  microcomputer  and  a  Unimate  500  robot 
[27].  The  structure  of  the  entire  system  is  shown  in  Fig.  2.7.  Vision 
processing  software  is  developed  to  perform  connectivity  analysis,  iden¬ 
tifying  discrete  objects  in  the  scene,  calculation  of  the  geometric 
features  of  the  largest  object  and  the  object's  location  and  orien¬ 
tation.  The  vision-robot  system  is  capable  of  finding,  in  real  time,  a 
workpiece  in  the  field  of  view  with  random  position  and  orientation,  to 
identify  and  inspect  it,  and  to  bring  it  to  a  specified  location  with 
predetermi ned  orientation. 

2.3.4  The  Robot-Vision  System  of  the  University  of  Rhode  Island 

This  vision  system  consists  of  three  GE  TN200  cameras  and  their  GE 
PN2110A  interfaces  [28].  The  pixel  intensity  data  and  timing  signals 
are  routed  through  a  DMA  (Direct  Memory  Access)  channel  to  computer 
memory.  A  Grinnell  GMR-26  raster  graphics  display  generator,  in  addi¬ 
tion  to  the  ability  to  generate  vectors,  rectilinear  graphics,  alpha- 
numerics  and  special  characters,  allows  the  simultaneous  display  of  up 
to  4  camera  images.  One  camera  is  installed  on  the  robot  and  is  used  as 
the  primary  imaging  device  to  pick  up  workpieces  randomly  placed  in  a 
bin  [29].  The  second  camera  is  located  at  a  fixed  location  and  is 
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used  to  determine  the  position  and  orientation  of  the  workpiece  held  by 
the  robot  [30].  The  third  camera  is  a  spare  one.  While  the  three  sys- 
tans  described  earlier  deal  mainly  with  two-dimensional  image,  URI's 
system  focuses  on  solving  three-dimensional  problems.  The  position  and 
orientation  of  a  workpiece  is  determined  using  a  stereo  vision  tech¬ 
nique.  The  workpiece  is  held  by  the  robot  and  is  presented  to  the 
camera.  After  one  picture  is  taken  and  analyzed,  the  arm  moves  to  a 
different  location  from  where  a  second  picture  is  acquired.  Corres¬ 
ponding  feature  points  in  both  pictures  are  identified  and  triangul ation 
is  performed  to  decide  their  coordinates.  This  information  is  then 
compared  with  the  model  of  the  workpiece  to  determine  its  pose. 

2.4  Vision  Systems  Applied  to  Mobile  Vehicles 

The  robots  employed  in  the  systems  mentioned  so  far  all  operate  at 
a  fixed  station  which  does  not  move,  and  in  most  industrial  appli¬ 
cations,  the  working  environment  of  a  robot  is  well  structured  in  the 
sense  that  care  has  been  taken  to  avoid  the  situations  in  which  a  robot 
runs  into  unexpected  objects  by  accident.  Therefore,  the  contents  of  a 
scene  viewed  from  the  vision  system  may  vary  frequently,  but  the  sur¬ 
rounding  as  a  whole  does  not  change  signi ficantly .  However,  this  is  not 
true  for  a  mobile  vehicle.  Allowing  a  machine  to  move  adds  another  six 
degrees  of  freedom  to  the  entire  system  and  certainly  make  things  more 
coinpl  icated.  It  would  be  helpful  to  learn  how  a  vision  system  is  incor¬ 
porated  as  part  of  a  problem  solver  to  deal  with  varying  environments. 
For  this  reason,  five  existing  mobile  vehicles  with  vision  capability 
will  be  examined  below. 


2.4.1  The  Hopkins  Beast 


This  machine  was  built  in  the  early  1960  at  the  Johns  Hopkins 
University  [31].  It  resembles  a  small  garbage  can  on  wheels,  and  its 
navigation  system  is  based  upon  sonar  measurements  that  attempt  to  keep 
it  centered  as  it  moves  between  two  parallel  walls.  Its  eye,  a  combi¬ 
nation  of  photocells,  masks,  lenses,  and  circuits,  is  designed  for 
detecting  electric  outlet  coverplates  since  it  is  a  battery-operated 
system  and  has  to  recharge  its  batteries  frequently.  Thus  as  the  Beast 
sees  it,  the  world  consists  of  only  two  things:  coverplates  and  every¬ 
thing  else.  As  it  moves  along  a  hallway,  it  would  suddenly  stop,  move 
over  to  an  electric  outlet  on  the  wall,  stick  out  its  plug,  feel  for  the 
precise  position  of  the  outlet,  and  plug  itself  in.  That  is  about  all 
that  Beast  can  do.  It  is  said  that  its  only  purpose  in  "life"  is  to 
keep  its  batteries  charged.  However,  it  is  a  self-contained  mobile 
vehicle  since  there  is  no  cable  or  radio  link  to  any  computer,  power 
supply  or  human-operated  terminal. 

2.4.2  The  SRI  SHAKE Y  Robot 

This  machine  possessed  a  TV  camera,  tactile  and  distance  sensors, 
and  was  controlled  by  a  radio  link  [31].  It  was  built  in  an  attempt  to 
investigate  research  issues  in  the  fields  of  machine  perception  and 
problem  solving.  The  first  version  of  SHAKEY  was  completed  in  late 
1969.  Problems  soon  flourished  after  experiments  had  been  carried  out. 
First,  its  activity  was  limited  to  the  simplest  tasks  due  to  the  inade¬ 
quacy  of  the  computer,  which  was  a  XDS-940  with  64,000  24-bit  words  of 
slow  core  memory.  Second,  the  multiple  representations  of  the  enviro- 
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nment,  including  (1)  a  TV  picture  representation  stored  and  processed  in 
data  array,  (2)  a  model  of  the  floor  of  the  room  which  was  segmented 
into  grids  and  labeled  as  occupied,  unoccupied,  and  unknown  regions,  and 
(3)  one  that  consisted  of  symbolic  axioms  of  predicate  calculus  to  be 
used  by  problem- sol ving  oriented  subsystems,  were  awkward  and  made  it 
very  difficult  to  update  as  the  world  changed.  Third,  the  software 
developed  did  not  allow  the  utilization  of  sensors  to  check  progress, 
and  motion  execution  was  completely  open  loop,  thus  a  slight  error  some¬ 
where  during  execution  would  result  in  a  crash  of  the  whole  system. 

The  second  version  was  completed  by  the  end  of  1971.  The  computer 
system  was  replaced  by  a  more  powerful  PDP-10  with  200,000  36-bit  words 
of  faster  core  memory.  The  representation  was  simplified  to  a  single 
set  of  axioms  of  first  orler  predicate  calculus.  The  software  was 
reorganized  to  assune  a  hierarchical  structure  containing  four  major 
levels:  Low-Level  Action,  Intermediate-Level  Actions,  the  STRIPS 
problen- sol  ving  system,  and  the  MACROP  executive  program.  After  these 
modifications,  SHAKEY  was  able  to  roll  about  in  a  laboratory  environment 
consisting  of  several  interconnected  rooms  populated  with  wooden  blocks. 

2.4.3  The  Stanford  CART  Vehicle 

The  CART  was  an  electrical  vehicle  remotely  controlled  over  a  CB 
radio  link  by  a  P0P-KL1Q  computer.  The  vision  unit  was  a  black  and 
white  TV  camera  whose  picture  was  broadcast  over  a  UHF  channel  and  digi¬ 
tized  by  the  computer.  The  goal  of  this  system  was  to  utilize  a  stereo 
vision  system  to  guide  a  robot  to  move  through  a  cluttered  environment, 
avoid  obstacles,  navigate  to  desired  locations,  and  build  a  description 
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of  its  environment.  With  only  one  camera  on  the  vehicle,  stereo  vision 
was  accomplished  by  taking  two  pictures  with  the  camera  moved  on  a  hori¬ 
zontal  silder  to  one  of  nine  different  locations.  The  processing  of  the 
stereo  pairs  was  done  as  follows:  First,  a  special  operator  found  small 
features  with  high  information  content  in  the  first  picture.  Then,  a 
binary  search  correlator  found  the  corresponding  points  in  the  other 
picture.  After  these  matched  points  were  identified,  the  distances  to 
the  corresponding  points  in  three-dimensional  space  were  computed  using 
the  known  camera  model.  This  information  was  then  transformed  into  a 
coordinate  system  approximately  aligned  with  the  horizontal  surface.  A 
ground  surface  finder  was  used  to  find  the  ground  for  portions  of  the 
scene,  and  points  which  were  sufficiently  above  the  ground  surface  were 
assumed  to  lie  on  objects.  This  project  is  currently  being  pursued  by 
Moravec  at  Carnegie-Mel Ion  University,  where  a  new  vehicle,  the  CMU 
Rover,  is  under  construction  [11]. 

2.4.4  The  JPL  Robot  Vehicle 

The  central  component  of  this  system  was  a  scanning  Laser  Range¬ 
finder  (LRF) ,  which  was  part  of  a  "vision"  system  that  also  included  two 
TV  cameras  and  a  minicomputer.  The  purpose  of  LRF  was  to  provide  a 
means  for  geometrical,  three-dimensional  location  of  objects  or  surfaces 
in  the  neighborhood  of  the  robot  vehicle  for  use  as  input  information  to 
the  autonomous  control  system.  The  instrument  beam,  a  gallium  aluminum 
arsenide  solid-state  laser,  was  directed  at  specified  points  or  scanned 
over  a  specified  area  under  computer  control.  The  detector  was  a  type 
C31034  photo-multiplier  having  a  gallium  aluminum  photo  surface  with 
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special  sensitivity  to  match  the  laser  emission.  The  range  data  was 
measured  by  using  the  time-of-fl ight  method.  The  dual  TV  cameras  were 
used  as  a  backup  system  to  the  LRF  and  could  also  provide  redundant 
i n formation. 

2.4.5  The  RP I  Mars  Rover 

This  system  employed  laser  triangulation  to  compute  the  distance  of 
an  object  point.  The  laser  beam  was  focused  onto  a  multiple-sided 
rotating  mirror  which  had  two  degrees  of  freedom  of  motion,  and  then 
reflected  toward  a  certain  direction.  Meanwhile,  the  returned  ray  from 
a  terrain  point  was  detected  by  a  mul ti -el ament  linear  array  of  photo¬ 
sensors.  Since  the  detectors,  laser  and  mirror  were  all  permanently 
installed  on  a  mast,  their  geometric  relationships  were  known.  The 
rotating  of  the  mirror  was  under  computer  control.  Thus  the  position  of 
the  mirror  at  every  instant  was  available  as  well  as  the  angle  of  the 
emitted  laser  ray.  The  detectors  provided  the  angle  of  the  returned 
bean.  The  distance  of  the  corresponding  terrain  point  was  computed 
using  this  information.  Each  distance  was  associated  with  an  elevation 
and  an  azimuth  angle  which  indicated  the  position  of  the  mirror.  By 
incrementing  both  angles  gradually,  an  area  was  scanned  and  a  range 
matrix  was  constructed.  These  range  data  were  further  processed  to 
characterize  the  terrain. 

2.5  Techniques  for  Distance  Measurements 

During  the  Fol low-The-Leader  (FTL)  operation,  the  Hexapod  needs  to 
locate  a  specified  foothold  and  refer  it  to  the  current  body  coordinate 
system.  In  essence,  Hexapod  must  be  able  to  measure  the  distance  vector 
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between  that  foothold  and  the  center  of  its  body.  Two  types  of  schemes 
exist  for  this  purpose;  one  based  on  the  trigonometry  of  triangul ation 
and  the  other  based  on  the  time-of-fl ight  of  light  (or  sound).  They  are 
discussed  below. 

2.5.1  Triangul ation  Methods 

In  general,  three  elements  are  involved  in  the  triangul  ation 
method.  One  is  the  object  point  0  whose  distance  is  to  be  measured,  and 
the  other  two  can  be  both  receivers  (Fig.  2. 8. a)  or  one  transmitter  and 
one  receiver  (Fig.  2.8.b).  Three  vectors,  denoted  as  A,  B,  and  C,  are 
defined  between  them  and  form  a  triangle;  thus  the  terminology  "tri angu¬ 
lation"  was  originated.  Vectors  A  and  B  represent  optical  paths  and  C 
is  the  displacanent  vector  between  receivers/transmi tter.  Once  these 
vectors  are  known,  the  range  can  be  calculated  by  a  simple  trigonometric 
formul a. 

If  two  receivers  are  employed,  it  is  called  stereo  vision  and  is 
achieved  either  with  two  cameras  or  one  camera  at  two  locations.  The 
receivers  are  passive  devices;  however,  the  illuminating  mechanism  can 
be  passive  or  active.  For  instance,  an  active  laser  beam  designator  is 
used  in  this  research  work  to  specify  a  foothold  so  that  the  binocular 
vision  system  may  perceive  it,  while  applications  which  analyze  natural 
scenes  rely  on  natural  lighting  only  and  are  considered  to  have  a  pas¬ 
sive  light  source.  Light  sources  may  also  be  arranged  in  a  certain  way 
to  produce  desired  lighting  conditions.  This  technique  is  known  as 
"structured  light  source  method,"  as  demonstrated  by  the  CONSIGHT  system 
[25],  and  is  also  considered  as  active.  The  most  difficult  problem  in 
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Figure  2.8  Triangulation:  (a)  with  two  receivers-- 

stereo  vision  method; 

(b)  with  one  transmitter  and 
and  one  receiver — light- 
projecting  method. 


stereo  vision  is  to  find  the  corresponding  points  in  the  two  pictures. 
Although  solutions  to  this  problem  have  been  proposed  by  several  re¬ 
searchers  [32],  there  is  still  need  for  a  more  reliable  and  fast  method 
to  perform  the  computation. 

If  the  combination  of  a  transmitter  and  a  receiver  is  adopted,  it 
is  called  the  projected-! ight  method.  The  laser  triangulation  employed 
by  the  RP I  Mars  Rover  falls  into  this  catagory.  Other  systems  project 
sheets  of  light  onto  the  scene  by  means  of  a  slit  projector  [33],  a 
laser  beam  diverged  by  a  cylindrical  lens  [34],  or  a  collimated  light 
beam  rotated  quickly  by  a  mirror  [35].  The  correspondence  problem  is 
eliminated  in  this  scheme. 

Both  methods  suffer  from  the  following  drawbacks:  missing  data 
seen  by  one  but  not  the  other,  and  unsatisfactory  accuracy  for  points 
that  are  far  away. 

2.5.2  Time-Of-Fl  ight  Methods 

In  the  time-of-fl ight  method,  the  range  is  determined  from  the  time 
needed  for  the  light  to  travel  from  the  transmitter  to  the  target  and 
back.  If  an  ultrasonic  signal  is  used  instead  of  light,  it  can  be 
implemented  as  a  proximity  sensor.  Such  sensors  have  been  designed  and 
built  for  the  OSU  Hexapod  and  have  been  mounted  on  each  leg  to  measure 
the  distance  from  the  foot  tip  to  the  terrain  surface  [36],  Laser  scan¬ 
ners  have  been  used  intensively  for  measuring  range  [12,37].  Depending 
on  the  measurable  range,  accuracy,  type  of  laser,  and  typical  appli¬ 
cations,  laser-based  techniques  are  further  classified  into  inter¬ 
ferometric,  beam  modulation  telemetry,  and  pulse  time  of  flight.  A 
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detailed  description  and  comparison  of  these  approaches  were  given  in 
[38].  The  main  shortcoming  of  laser  scanner  schemes  to  date  is  that 
they  are  too  slow,  especially  if  the  target  is  dark  [37].  Such  systems 
also  require  sophisticated  electronics  for  signal  conditioning  (modu¬ 
lation,  demodulation,  filtering,  phase  detection,  etc.)  and  tend  to  be 
more  expensive  as  well  as  more  complex  compared  to  those  systems  which 
use  triangul  ation. 

2.6  Previous  Work  on  the  OSU  Hexapod 

Before  the  Hexapod  was  constructed,  Sun  [39]  investigated  the 
theoretical  aspects  of  legged  locomotion  systems.  Among  other  things, 
hexapod  gait  was  studied  in  depth.  Orin  [40]  conducted  a  simulation  to 
study  the  interactive  control  of  the  vehicle.  The  kinematic  and  dynamic 
equations  were  also  derived.  The  Hexapod  was  built  in  1977,  with  Jaswa 
[41]  handling  cne  mechanical  design  and  instrumentation.  Buckett  [42] 
designed,  implemented  and  analyzed  tne  electrically  powered  joint  actu¬ 
ator  system.  Chao  [43]  studied  the  feasibility  of  a  mul ti processor  con¬ 
trol  system  for  the  Hexapod  and  developed  much  of  the  software  enabling 
it  to  walk  under  interactive  control  from  an  operator.  Briggs  [44] 
designed  and  implemented  the  data  link  between  the  Hexapod  and  a  PDP- 
11/45  (now  1 1/70)  computer  to  facilitate  data  communication.  He  also 
installed  a  vector  force  sensor  on  one  leg  to  examine  the  effect  of 
active  compliance.  Wanawisan  [45]  built  a  pentaprocessor  using  5  LSI-11 
units  to  control  the  Hexapod.  Pugh  [46]  added  a  force  sensor  to  each 
leg,  installed  a  gyroscope  which  provided  body  attitude  information,  and 
devised  algorittms  utilizing  this  information  to  achieve  attitude 
control  and  active  compliance.  As  a  result,  the  Hexapod  was  able  to 
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walk  over  rough  terrain.  Fig.  2.9  shows  a  picture  of  the  QSU  Hexapod 
before  the  vision  system  was  installed. 

2.7  Summary 

This  chapter  presently  a  brief  review  of  the  state  of  the  art  of 
computer  vision  systems  and  their  application  to  industrial  robots  and 
mobile  vehicles. 

A  computer  vision  system,  in  general,  is  composed  of  an  image  sen¬ 
sor,  an  image  processor,  and  a  set  of  image-processing  algorithms  stored 
in  a  host  computer.  Four  types  of  image  sensors,  image  dissectors, 
vidicon  cameras,  CCG  cameras  and  CIO  cameras,  were  di scribed.  Solid- 
state  TV  cameras,  due  to  various  appealing  characteristics,  are  expected 
to  dominate  in  future  applications.  Special-purpose  image  processors 
are  designed  to  handle  the  large  volume  of  image  data  so  that  the  over¬ 
all  operational  speed  can  be  upgraded,  image  data  can  be  properly  stored 
without  occupying  the  memory  space  of  the  host  computer,  and  the  host 
computer  can  concentrate  on  the  execution  of  high-level  algorithms.  The 
purpose  of  image-processing  algorithms  is  to  analyze  a  scene  and  to 
extract  useful  information  from  it. 

Four  existing  robot-vision  systems,  the  General  Motors  CONSIGHT 
system,  the  UNIVISION  system,  the  Westinghouse  system,  and  the  URI  sys¬ 
tem  were  examined.  In  all  cases,  a  vision  system  has  been  utilized  as  a 
visual  sensor  to  guide  a  robot  to  perform  certain  tasks. 

As  for  mobile  vehicles,  vision  systems  are  used  to  survey  the 
nearby  environment,  to  generate  a  terrain  map,  and  to  navigate  the 
vehicle  through  cluttered  surroundings,  as  examplified  by  the  Hopkin's 
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Beast,  the  SRI  SHAKEY,  the  Stanford  Cart,  the  JPL  Rover  and  the  RPI  Mars 
Rover. 

Two  different  schemes  for  measuring  distance  were  also  discussed. 
The  triangul  ation  method  is  based  on  the  computation  of  three  spatial 
vectors,  while  the  time-of-fl ight  approach  depends  on  the  measurement  of 
the  time  interval  between  the  outgoing  and  the  returning  signals. 
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Chapter  3 


FUNCTIONAL  DESCRIPTION  OF  THE  BINOCULAR  VISION  SYSTEM 
3.1  Introduction 

The  superior  off-road  mobility  of  a  biological  system  can  be 
attributed  to  the  following  factors: 

(1)  A  body  structure  which  is  suitable  for  locomotion  in  rough 
terrain, 

(2)  A  vision  system  (eyes)  that  provides  environmental 
i  nfonnati  on, 

(3)  A  decision-making  and  coordination  capability  (brain). 

The  development  of  artificial  legged  machines  provides  researchers  with 
an  experimental  body  structure  similar  to  that  of  an  animal.  However, 
without  the  functions  of  the  eyes  and  the  brain,  the  capability  of  a 
walking  machine  is  highly  restricted.  To  improve  the  versatility  of 
such  a  vehicle,  a  vision  system,  which  functions  as  the  eyes,  and  a  com¬ 
puter  system,  which  serves  as  a  brain,  are  needed.  In  general,  a  vision 
system  can  be  utilized  as  a  terrain  sensor  and  will  enable  a  machine  to 
acquire  vital  environmental  data.  Based  on  this  i nfonnati on,  computer 
programs  can  analyze  the  terrain,  select  a  safe  route,  and  guide  the 
machine  toward  a  destination.  It  will  be  shown  in  this  chapter  that 
with  a  vision  system,  one  can  implement  Follow-The-Leader  operation  for 
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legged  locomotion,  which  is  an  important  step  toward  the  realization  of 
a  fully  autonomous  machine. 

In  Section  3.2,  the  Fol low-The-Leader  control  mode  is  introduced. 
Problems  encountered  in  realizing  it  are  explored  and  possible  solutions 
are  proposed.  Section  3.3  describes  the  function  of  each  element  in¬ 
volved  in  the  proposed  vision  system.  Human  interactions  are  covered  in 
Section  3.4.  Operational  and  testing  procedures  are  given  in  Section 
3.5  so  that  the  normal  function  of  the  vision  system  can  be  checked. 
Section  3.6  examines  the  program  which  controls  the  scanning  operations 
of  the  vision  system  in  detail. 

3.2  Fol low-The-Leader  Control  Mode 

The  successful  implementation  of  a  supervisory  control  scheme  [5] 
relieved  a  human  operator  from  the  cumbersome  task  of  manual  coordi¬ 
nation  of  the  motions  of  all  joints  as  exemplified  by  the  G.E.  Quadru¬ 
ped  Vehicle.  Nevertheless,  even  in  its  most  advanced  realization,  joy¬ 
stick  control  operation,  human  intelligence  is  still  requi red  to  provide 
the  desired  speed  and  direction. 

The  next  step  toward  the  goal  of  a  completely  autonomous  machine  is 
the  so-called  Fol low-The-Leader  control  operation.  In  this  mode,  the 
operator  specifies  footholds  for  the  front  foot  on  each  side  and  the 
others  follow  in  their  footprints  as  the  vehicle  moves  forward.  Human 
intelligence  is  incorporated  to  deal  with  route  selection  and  obstacle 
avoidance.  However,  the  vehicle  figures  out  for  itself  the  necessary 
speed,  turning  rate,  direction  and  movements  so  that  the  foot  tip 
touches  down  at  the  specified  location.  This  feature  is  not  found  in 
the  joystick  control  mode. 
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A  number  of  questions  arise  as  an  attempt  is  made  to  implement 
Fol low-The-leader  control.  For  instance,  one  needs  to  know  how  a  foot¬ 
hold  is  represented,  how  it  can  be  generated  by  an  operator,  and  how  it 
is  recognized  by  the  vehicle.  After  recognizing  a  foothold,  the  machine 
has  to  know  where  it  is  with  respect  to  the  current  position.  Knowing 
where  a  foodhold  is,  it  then  has  to  figure  out  how  to  move  so  that  one 
of  its  front  feet  is  placed  down  at  that  point.  Also,  the  nature  of  the 
interaction  between  the  operator,  the  vehicle,  and  other  elements  inv¬ 
olved,  has  to  be  well  defined. 

Since  footholds  are  actually  surface  points  of  a  terrain,  it  is 
natural  to  refer  then  by  their  spatial  coordinates  (X,Y,Z).  One  possi¬ 
ble  way  to  accomplish  the  man-machine  interaction  as  regards  to  the 
generation  and  perception  of  footholds  is  to  employ  a  vision  system. 

The  operator  could  indicate  a  foothold  to  the  vehicle  by  using  an  opti¬ 
cal  medium  and  the  latter  would  recognize  it  through  an  optically  sensi¬ 
tive  instrument.  It  is  well  known  that  a  laser  beam  is  monochromatic, 
highly  directional,  and  can  be  collimated  into  a  narrow  beam.  These 
characteristics  make  it  an  ideal  light  source  for  this  application. 
Solid-state  TV  cameras,  on  the  other  hand,  have  such  attractive  features 
as  high  resolution,  high  operational  speed,  small  volume,  and  can  be 
easily  interfaced  with  most  digital  instruments.  Thus  it  was  decided 
that  a  laser  bean  designator  be  used  as  the  foothold  indicator  and  TV 
cameras  be  utilized  as  the  foothold  sensing  devices. 

To  know  where  a  foothold  is  introduces  the  necessity  of  measuring 
its  spatial  coordinates.  This  can  be  done  by  using  the  time-of-fl ight 
method  or  by  trigonometry.  At  the  time  this  work  was  initiated,  a 
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dedicated  and  expensive  system  based  on  a  time-of-fl ight  laser  scanner 
was  under  construction  by  ERIM  (Environmental  Research  Institute  of 
Michigan)  to  be  used  with  the  new  vehicle  (ASV)  to  create  a  terrain  map 
[47].  It  was  decided  that  alternatives  ought  to  be  explored  so  that 
advantages  and  drawbacks  of  different  approaches  would  be  understood. 
Hence,  the  triangul ation  method  was  chosen  for  the  research  of  this 
wo  rk . 

Image  data  acquired  by  a  single  camera  provides  two-dimensional 
information  only.  In  planning  a  movement  toward  a  foothold,  however,  it 
is  necessary  to  know  the  three-dimensional  spatial  coordinates  of  that 
point.  For  applications  using  cameras,  coordinate  reconstruction  is 
typically  done  by  triangul ation  [32].  Furthermore,  it  can  be  done  by 
using  either  one  or  two  cameras.  In  a  single  camera  system,  it  is 
necessary  to  take  at  least  two  pictures  from  two  different  views.  To 
achieve  this,  either  the  object  has  to  move  to  a  new  location  as  in  the 
URI  system  [28],  or  the  camera  should  take  a  new  position  as  demon¬ 
strated  by  the  Stanford  Cart  [11].  In  the  vehicle  guidance  application, 
the  object  points  are  terrain  surface  points  which  can  not  be  moved 
around,  thus  the  former  approach  is  not  applicable.  The  latter  method, 
which  requires  the  camera  to  move  to  a  second  location  after  one  picture 
is  taken,  not  only  is  inherently  slower  than  a  binocular  system  due  to 
the  time  needed  to  translate  the  camera,  but  also  demands  additional 
mechanism  to  relocate  the  camera.  Thus,  a  binocular  system  was  deemed 
more  desirable  and  was  adopted. 
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3.3  Description  of  Components 
3.3.1  The  Overall  System 

Five  elements  are  involved  in  the  overall  system:  a  human  oper¬ 
ator,  the  Hexapod,  a  vision  system,  a  laser  designator  and  a  P0P-11/70 
computer.  The  block  diagram  in  Fig.  3.1  shows  the  relationship  between 
these  elements. 

The  responsibility  of  the  human  operator  is  to  analyze  the  ter¬ 
rain  and  select  a  proper  foothold  for  the  Hexapod.  This  is  achieved  by 
manually  adjusting  the  position  and  orientation  of  a  laser  head  so  that 
the  laser  beam  is  aimed  at  a  desired  surface  point.  The  operator  also 
interacts  with  the  vision  system  by  indicating  that  a  desired  foothold 
i s  avail  able. 

The  Hexapod  is  a  six-legged  machine  built  in  1977  as  an  experi¬ 
mental  vehicle  for  studying  locomotion,  man-machine  interaction,  terrain 
adaptability,  fuel  efficiency,  etc.  Each  leg  has  three  joints  which  are 
driven  by  individual  motors  and  every  joint  is  equipped  with  one  poten¬ 
tiometer  and  one  tachometer  to  feed  back  joint  angle  and  joint  rate 
information,  respectively .  Commands  to  the  motors  are  generated  by  the 
11/70.  Communication  between  the  Hexapod  and  the  11/70  is  facilitated 
by  a  64-channel  Data  Link  [44] .  A  vertical  gyro  on  board  can  provide 
pitch  and  roll  angles  of  the  Hexapod  body.  Recently,  force  sensors  have 
been  added  to  all  legs  to  accomplish  attitude  and  altitude  regulation 
over  irregular  terrain  [46]. 

The  PDP-11/70  computer  provides  the  computional  power  needed  for 
real-time  control  of  the  Hexapod.  This  is  done  by  executing  a  motion 
planning/execution  program.  A  serial  link  exists  between  the  11/70  and 
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the  vision  system.  Through  this  channel,  image  data  acquired  by  the 
vision  system  is  transmitted  to  the  11/70  for  further  processing  and  the 
11/70  can  send  commands  to  control  the  scanning  operations  of  the  vision 
system. 

The  laser  designator  is  used  to  generate  footholds  by  projecting 
a  laser  beam  at  desired  surface  points.  It  includes  a  5  mW  Hel liun-Neon 
laser  head  (Ealing  Corporation,  #25-0860)  and  a  power  supply  unit 
(Ealing  Corporation,  #25-0894),  which  are  shown  in  Fig.  3.2.  When 
selecting  a  laser  head  for  this  application,  size  and  power  level  are 
the  two  main  considerations.  Both  a  2mW  laser  head  (282  mm  long,  31.75 
mm  in  diameter,  weight  of  0.2  Kg)  and  a  5  mW  one  (411  mm  long,  44.2  mm 
in  diameter,  weight  of  0.35  Kg)  were  tested.  While  the  former  has  the 
advantage  of  size,  its  lower  power  level  results  in  lower  pixel  inten¬ 
sity,  and  consequently  the  signal -to-noise  ratio  suffers.  Since  it  is 
highly  desirable  to  have  a  high  noi se-rejection  threshold,  a  5  mW  unit 
was  chosen.  The  laser  head  is  mounted  on  a  tripod  to  make  operations 
convenient. 

The  vision  system  consists  of  two  camera  heads,  two  camera  con¬ 
trollers,  two  wide-angle  lenses,  a  special  purpose  interface  circuit, 
two  broad-band  optical  filters,  two  filter  adaptors  and  a  power  supply 
unit.  They  are  described  in  the  following  sections. 

3.3.2  Camera  Head  and  Camera  Control  Unit 

The  TN  2500  camera  head  contains  a  Charge-Injection-Device  (CID) 
image  sensor,  the  logic  and  drive  circuits  used  to  scan  the  CID,  and 
pre-amplification  circuits.  The  CID  makes  use  of  a  two-dimensional 
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arr^y  of  coupled  metal-oxide-silicon  (MOS)  capacitors  to  collect  and 
store  the  photo  generated  charges.  The  pixels  of  the  CID  are  arranged 
in  244  rows  with  248  pixels  in  a  row. 

The  CCU  (Camera  Control  Unit)  consists  of  a  timing  circuit  board, 
which  generates  all  of  the  required  digital  clocking,  gating  and  syn¬ 
chronization  functions,  and  a  signal  processor  circuit  board,  which  per¬ 
forms  all  of  the  required  video  signal  conditioning.  It  also  provides 
signals  for  external  interface  through  a  25  pin  connector.  The  block 
diagram  in  Fig.  3.3  shows  the  functional  relationship  between  the  camera 
head,  CCU,  and  external  devices,  and  a  picture  of  the  physical  devices 
i s  given  in  Fig.  3.4. 

Output  signals  B1  to  B8  (Video  Bit  I  to  Video  Bit  8)  provide  the 
8-bit,  gray-scaled  digital  video  output,  with  B1  be  the  most  significant 
bit  and  B8  the  least  significant  bit.  The  elementary  clock  signal  is 
given  at  pin  ERC  (Element  Rate  Clock).  A  negative  transition  of  ERC 
denotes  the  presentation  of  new  digital  video  data.  In  addition  to  ERC, 
other  timing  signals  are  also  available,  and  each  carries  different  in¬ 
formation.  A  negative  transition  of  EF  (Even  Field  Flag)  denotes  the 
start  of  the  even  field  video  presentation,  while  a  negative  transition 
of  OF  (Odd  Field  Flag)  denotes  the  start  of  the  odd  field  video  presen¬ 
tation.  A  high  level  of  SBLNK  (Synchronized  Blinking)  coincides  with 
the  presence  of  digital  video  output,  and  a  low  level  coincides  with  the 
horizontal  or  vertical  blanking  interval.  No  digital  video  information 
is  presented  when  this  signal  is  low.  A  positive  transition  of  VSYNC 
(Vertical  Sync)  denotes  the  start  of  each  frame  when  the  camera  is 
operated  in  the  sequential  mode,  and  a  positive  transition  of  HSYNC 
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ram  Showing  Relationships  Between  Camera  Head,  Camera  Control 
External  Interface. 


3.4  Camera  Head  and  Camera  Control  Unit 


(Horizontal  Sync)  denotes  the  start  of  each  horizontal  video  line,  ."he 
5X  (5X  Clock)  is  a  supplementary  clock  source  whose  rate  (22.5225  MHz) 
is  5  times  of  that  of  ERC.  Timing  diagrams  Fig.  3.5  and  Fig.  3.6  illus¬ 
trate  the  relationships  between  these  timing  signals. 

The  input  signals  are  mainly  used  to  regulate  the  operation  of  the 
CCU.  A  low  command  at  G1  (Video  Output  Enable)  enables  the  digital 
video  output  and  a  high  command  forces  B1  through  B8  to  a  high  impedance 
state.  A  low  level  at  G2  (Synchronizing  Clock  Output  Enable)  enables 
the  clock  outputs.  A  high  level  forces  all  clock  outputs  into  a  high 
impedance  state.  A  high  level  in  input  pin  1/244SEQ  (Interlace  or  244 
Sequential)  forces  the  camera  to  operate  in  the  interlaced  mode.  A  low 
level  causes  the  video  to  be  output  in  a  244  line  fully  sequential  mode. 
When  the  input  line  IIG  (Injection  Inhibit  Gate)  is  high,  the  camera 
will  integrate  and  inject  signal  charge  once  per  frame.  Changing  it  to 
a  low  level  will  cause  extended  integration  of  the  signal  charge  and 
inhibit  injection.  The  input  pins  GI,  G2,  IIG,  and  I/244SEQ  are  con¬ 
trolled  externally  by  using  a  dip  switch.  For  normal  operation,  GI,  G2 
and  I/244SEI)  are  grounded  and  IIG  is  pulled  high  so  that  the  digital 
video  and  clock  signals  are  available  all  the  time,  the  CCU  is  operating 
in  a  sequential  mode,  and  every  pixel  is  injected  for  each  frame. 

The  individual  pixels  are  read  at  a  rate  of  4.5  MHz,  which  cor¬ 
responds  to  one  pixel  every  222  nsec.  Because  of  this  high  clock  rate, 
general  purpose  microprocessors  are  not  fast  enough  to  process  intensity 
data  of  each  pixel,  and  special  purpose  hardware  is  needed.  A  pixel  is 
cleared  as  it  is  read  to  initiate  the  reintegration  of  charge  for  the 
next  frame,  except  for  the  case  in  which  the  IIG  (Injection  Inhibit 
Gate)  is  enabled.  A  pixel  will  continue  to  integrate  charge  without 
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injection  as  long  as  the  IIG  is  enabled.  This  allows  a  low  light  image 
to  be  accumulated  over  several  frames. 

The  camera  system  can  provide  either  an  interlaced  or  sequential 
raster  scan  output.  In  the  interlaced  mode,  each  CID  image  line  is  read 
and  displayed  twice.  Odd  numbered  lines  make  up  field  1  and  even 
numbered  lines  make  up  field  2.  Subsequently,  these  odd  and  even  fields 
are  interlaced  at  a  rate  of  60  Hz  and  the  resultant  frame  rate  is  30  Hz. 
This  significantly  reduces  the  flicker  of  the  picture  on  the  monitor. 

In  the  sequential  mode,  a  frame  is  composed  of  only  one  field.  Thus, 
the  field  rate  equals  the  frame  rate,  which  is  30  Hz.  There  is  no 
interlacing  and  the  line  information  is  simply  displayed  sequentially. 
This  causes  a  noticeable  flicker  in  the  video  image. 

Although  the  interlaced  scanning  mode  produces  better  pictures,  it 
introduces  overhead  in  data  processing.  For  instance,  the  image  data 
has  to  be  stored  or  retrieved  in  an  interleaved  manner  in  order  to  gen¬ 
erate  a  correct  picture.  Such  overhead  would  not  exist  in  the  sequen¬ 
tial  mode  of  operation.  Therefore,  the  sequential  scanning  mode  is 
adopted  in  this  application. 

Detailed  descriptions  of  the  camera  head  and  the  CCU  are  given  in 

[48]. 

3.3.3  Camera  Locations  and  Wide-Angle  Lens 

In  this  binocular  vision  systen,  reconstruction  of  the  three- 
dimensional  coordinates  of  a  foothold  is  done  by  triangul ation.  To 
achieve  this,  a  foothold  must  be  "seen"  by  both  cameras,  i . e . ;  the 
foothold  must  be  within  the  "field  of  view"  of  both  cameras. 
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The  field  of  view  of  a  camera  system  depends  on  the  working  dis¬ 
tance  and  its  angle  of  view.  A  working  distance  is  defined  as  the 
distance  between  the  lens  center  and  a  focused  object  along  the  optical 
axis.  The  angle  of  view,  on  the  other  hand,  is  related  to  the  size  of 
the  image  sensor  and  the  focal  length  of  the  lens.  Fig.  3.7  shows  the 
relationship  between  focal  length  f,  sensor  size  i,  angle  of  view  2 e  and 
field  of  view  V.  It  i s  clear  that  any  light  source  outside  area  ACB 
will  not  be  sensed  by  the  camera.  Since 


tnerefo re, 

2  e  =  2tan'!  (^-)  (3.2) 

It  is  also  obvious  that  the  field  of  view  V  at  a  working  distance  of  d 
is  2d  tane  because 

v/2  i 

tan  e  =  ~  =  {—)  (3.3) 

d  2r 

thus 

V  =  2d  tane  (3.4) 

As  the  size  of  an  image  sensor  of  the  G.E.  TN25U0  camera  is  known 
(a. 78  mm  by  11.41  mm),  only  two  free  parameters  are  needed  to  specify 
a  field  of  view:  working  distance  and  focal  length  of  the  lens.  Since 
both  cameras  are  to  be  mounted  on  the  Hexapod  and  the  objects  to  be 
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Figure  3.7  Relationship  Between  Focal  Length, 
Sensor  Size,  Angle  of  View,  and 
Field  of  View. 


sensed,  footholds,  are  mostly  on  the  ground  surface,  the  working  dis- 

i 

;  tance  depends  on  the  elevation  of  the  Hexapod  with  respect  to  ground  and 

!  where  the  cameras  are  mounted  onto  the  Hexapod  body,  as  shown  in  Fig. 

3.8.  The  width  of  the  Hexapod  is  about  150  cm.  According  to  this 
measuranent,  a  field  of  view  of  2  m  is  needed  for  both  cameras  to  cover 
the  area  of  interest,  which  consists  of  points  reachable  by  the  front 
*  feet  in  one  step.  Having  decided  on  a  desired  field  of  view,  it  is  now 

necessary  to  find  out  an  appropriate  camera  location  and  a  suitable 
focal  length  which,  when  combined,  would  give  the  required  field  of 
view. 

Refer  to  Fig.  3.9  and  assune  that  a  camera,  with  an  angle  of  view 
of  2e,  is  located  at  ( X , Y > ,  where  -X0  <  X  <  XO  and  0  <  Y.  To  cover  the 
area  of  interest,  t-XG.XQ],  it  is  necessary  that  Z.8AC  <  2e .  For  the 
worst  case,  Z.BAC=2e,  where  2e  is  the  intersecting  angle  between  vectors 
Ad  and  AC  and  the  following  equation  holds: 

Ad  *  AC  =  cos2e  •  | AB I  *  I  AC  I  (3.5) 


where 


AB  =  (-X0-X,  -Y)  and  AC  =  (XO-X,  -Y ) . 

Substituting  A8  and  AC  into  Eq.  (3.5)  results  in 

(-X0-X,  -Y)  .  (XO-X,  -Y)  =  COS20  •  y"(X  +  XO)2  +  Y2  •  -yjT* XO)2  +  Y2 

(3.6) 
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Figure  3.8  Hexapod  Body  versus  Area  of  Interest. 


Figure  3.9  Locations  Giving  the  Required  Field  of  View 
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Multiplying  the  left  hand  side  terms  out  gives 

Y2+  x2-  XO2  =  COS29  •  Jlx  +  XO)2  +  Y2  •  (X  -  XO)2  +  Y2 

"  (3.7) 

Employing  Eq.  (3.1)  gives 

Y2  +  X2  -  XO2  =  cos2.[tan‘1(^-)]  •  -^Tx  +  XO)2  +  Y2  • 

V(X  -  XO)2  +  Y2  (3.8) 


Thus,  given  a  sensor  of  size  x  and  a  lens  of  focal  length  f,  there 
is  a  set  of  points  on  the  X-Y  plane  which  provides  the  desired  field  of 
view.  Fig.  3.10  shows  curves  of  these  set  of  points  for  a  9  mm  by  9  mm 
sensor  for  focal  lengths  of  2,  4,  6,  8,  and  10  mm,  respectively.  Theo¬ 
retically,  there  is  no  restriction  on  the  value  of  Y;  i.e.  the  height 
of  the  camera  mounting.  However,  from  a  practical  point  of  view,  it  is 
desirable  to  keep  the  camera  position  as  low  as  possible  to  avoid  mecha¬ 
nical  instability.  The  Hexapod  itself  is  about  75  cm  high  and  a 
decision  was  made  to  limit  camera  height  to  be  less  than  150  cm  to 
maintain  structural  stability.  These  two  considerations  imply  a  focal 
length  between  5  mm  and  7  mm.  Consequently,  two  6.5  mm  wide-angle 
lenses  were  purchased. 

Although  a  wide-angle  lens  provides  a  larger  field  of  view,  it  also 
introduces  considerable  distortion,  and  lens  calibration  is  required  to 
compensate  for  the  distortion.  This  topic  is  discussed  in  Section  4.4. 
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3.3.4  Optical  Filters 


Since  the  interface  circuit  is  designed  to  detect  only  one  target 
point,  any  ambient  light  source  whose  intensity  is  higher  than  the 
threshold  and  whose  location  is  scanned  earlier  than  the  desired  spot 
will  be  recognized  as  a  target,  thus  producing  a  wrong  result. 

Internal  thermal  noise  of  a  CIO  can  generally  be  handled  by 
choosing  a  proper  threshold.  External  noise,  such  as  sunlight  or 
fluorescent  lamp,  can  be  so  strong  that  it  is  impossible  to  exclude  them 
by  a  higher  threshold  because  the  desired  object  source  may  also  be 
blocked  out. 

One  alternative  to  reject  ambient  light  is  to  utilize  optical 
filters.  Since  the  desired  light  source  in  this  application  is  a 
Helium-Neon  laser  beam  which  is  monochromatic  and  has  a  wavelength  of 
632.8  nm,  it  is  expected  that  a  narrow-band  filter  would  allow  the  laser 
beam  to  reach  the  camera  while  filtering  out  most  of  the  unwanted  light 
sources.  One  such  filter  {Oriel  Corporation,  #5308)  with  a  bandwidth  of 
10  nm  was  attached  in  front  of  the  camera  lens  by  using  a  specially  de¬ 
signed  adaptor.  Although  it  blocked  out  most  of  the  ambient  light,  the 
following  problem  was  introduced.  The  passing  wavelength  was  specified 
as  632.8  nm.  However,  this  is  true  only  if  the  incident  light  is  normal 
to  the  filter  surface.  As  the  incident  light  deviates  from  the  normal 
direction,  the  effective  passing  wavelength  becomes  shorter.  This 
phenomenon  is  described  by  the  following  equation  [49]: 
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where 


a  =  angle  of  incidence, 

xa  =  peak  wavelength  at  angle  a, 

\n  =  peak  wavelength  at  normal  incidence, 
n  =  incidence  angle  shift  factor,  (n  has  a  value  of  2  for  the 
filters  that  are  used  in  this  system.) 

As  a  result,  the  effective  field  of  view  is  sharply  reduced. 

The  wide-angle  lens  used  has  a  9U  degree  angle  of  view,  with  45 
degrees  on  each  side  of  the  normal  direction.  If  the  reflected  light  is 
normal  to  the  filter  surface,  the  incident  angle  is  zero  and  the  effec¬ 
tive  peak  wavelength,  according  to  Eq.  (3.9),  is\n  (632.8  nm).  How¬ 
ever,  the  incident  angle  is  45°  in  the  worst  case,  which  produces  an 
effective  peak  wavelength  xa  of  560  nm  for  n  equals  to  2.  This  calls 
for  a  bandpass  filter  which  passes  light  of  wavelength  between  632.8  nm 
to  5o0  nm.  No  commercially  available  filter  has  exactly  this  desired 
characteristic.  To  compromise,  a  broad-band  filter  with  a  peak  wave¬ 
length  at  600  nm  and  a  ban<toidth  of  70  nm  (Oriel  Corporation,  #5760)  was 
used.  As  one  may  expect,  the  noise  rejection  capability  is  inferior  to 
the  narrowband  counterpart;  nevertheless,  the  decrease  of  the  field  of 
view  is  less  severe.  A  picture  showing  the  broad-band  filter  and  a 
adaptor  which  mounts  the  filter  onto  the  lens  is  provided  in  Fig.  3.11. 

Another  way  which  can  effectively  reject  ambient  light  is  to  reduce 
the  aperture  of  the  lens  to  a  minimum.  Fig.  3. 12. a  shows  a  picture 
taken  with  the  aperture  set  to  f/lb.  Fig.  3 . 1 2. b  shows  a  picture  of 
the  same  scene  taken  with  the  aperture  set  to  f/5.6.  It  is  obvious  that 
the  change  in  aperture  significantly  rejects  ambient  light.  The 
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Figure  3.11 


Optical  Filter 
Adaptor. 
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(a)  f/16 


(b)  f / 5.6 

Figure  3.12  Pictures  of  the  Same  Scene  Taken  with 
Different  Apertures. 
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reflected  laser  beam  is  also  attenuated  sharply,  and  the  size  of  the 
image  of  the  laser  beam  is  reduced  into  a  very  tiny  spot.  It  is  likely 
that  the  nonlinearity  of  the  internal  electronic  circuits  of  the  camera 
head  may  have  improved  the  signal-to-noise  ratio.  The  effect  of  the 
aperture  thus  is  similar  to  an  analog  threshold  device.  If  an  extremely 
strong  light  source  exists  in  the  field  of  view,  however,  it  would  not 
be  completely  rejected  even  if  the  aperture  is  set  to  a  minimum.  Such 
situations  should  be  avoided  for  the  vision  system  to  function 
properly . 

After  testing,  it  was  concluded  that  the  most  satisfactory  way  of 
rejecting  ambient  noise  while  maintaining  sufficient  reflected  laser 
intensity  is  to  use  a  minimum  aperture  (f/16)  without  optical  filters. 

3.3.5  Power  Supply  Unit 

The  power  supply  unit  provides  the  interface  circuit  board  with  the 
necessary  +5  and  ±12  volt  sources.  The  +5V  module  can  provide  up  to  3 
amperes,  while  the  ±12V  module  has  a  current  capability  of  1  ampere. 

3.3.6  Special  Purpose  Interface  Ci  rcui t 

The  primary  function  of  the  interface  circuit  is  to  receive  pixel 
intensity  data  and  timing  signals  from  both  of  the  camera  systems, 
determine  the  image  coordinates  of  an  object  point  on  each  camera  frame, 
and  then  transmit  these  coordinates  to  the  11/70  computer  through  an  RS- 
232  serial  link.  Fig.  3.13  shows  the  block  diagram  of  the  interface 
circuit.  It  contains  an  interface  controller,  two  image  coordinate  de¬ 
coders,  and  an  optical  isolation  unit.  Functions  and  implementations  of 
these  circuits  are  described  below. 
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3. 3.b.l  Interface  Controller 

The  interface  controller  is  composed  of  two  parts,  an  image  decoder 
controller  and  a  UAR/T  controller.  The  former  regulates  the  activities 
and  maintains  the  synchronization  of  the  two  image  coordinate  decoders. 
The  latter  supervises  the  communication  between  the  interface  circuit 
and  the  11/70  computer. 

3. 3. 6. 2  Image  Decoder  Controller 

As  shown  in  Fig.  3.13,  each  image  coordinate  decoder  receives 
timing  signals  from  its  associated  CCU  to  perform  image  detection. 

Since,  functionally  speaking,  the  two  CCU's  are  totally  independent,  the 
operation  of  the  image  decoders  would  be  asynchronous  to  e.ach  other. 

This  makes  the  acquisition  of  image  coordinates  difficult,  as  explained 
in  tne  following  example. 

Fig.  3.14  shows  a  relative  timing  diagram  of  the  two  CCU's  in  terms 
of  the  detection  of  an  image  within  a  frame.  Image  decoder  1  starts 
searching  at  the  beginning  of  a  frame,  marked  as  "a",  detects  an  image 
at  instant  b  and  ends  up  at  c,  where  a  new  frame  starts  and  the  same 
event  is  repeated  again.  Similar  activity  takes  place  in  image  decoder 
2  between  d  and  f.  The  question  arises  of  when  image  coordinates  should 
be  acquired  for  future  processing.  While  (XI, Yl)  is  available  through¬ 
out  period  [b,c],  (X2.Y2)  is  not  reacty  until  e.  At  this  point,  however, 
(XI, Yl)  is  no  longer  valid  because  a  new  cycle  has  already  begun. 

This  problem  can  be  eliminated  by  introducing  two  synchronizing 
signals,  START  and  COMPLETE.  Fig.  3.15  shows  the  timing  diagram  in 
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Figure  3.13  Organization  of  the  Binocular  Vision  System. 


Timing  Diagram  of  Two  Synchronized  CCU's  with  START  and  COMPLETE  Signals. 


which  START  and  COMPLETE  are  incorporated.  Notice  that  an  image  detect¬ 
ing  cycle  now  takes  more  than  one  frame.  The  next  paragraph  describes 
how  the  START  and  COMPLETE  signals  are  used  to  regulate  the  function  of 
the  image  coordinate  decoders  and  how  they  are  generated. 

To  synchronize  the  image  decoder  controller,  each  image  decoder  is 
assigned  four  states,  as  shown  in  Fig.  3.16.  Here,  the  state  variables 
of  decoder  1  are  Q1  and  QO,  and  those  of  decoder  2  are  Q3  and  Q2,  res¬ 
pectively.  After  POI  (Power-On  Initialization)  or  being  reset  exter¬ 
nally,  an  image  decoder  is  in  state  00.  It  stays  at  this  state  as  long 
as  START  is  low.  When  START  becomes  high,  it  enters  the  image  searching 
state  01  upon  the  leading  edge  of  the  next  coming  clock  signal  (EF) 
which  indicates  the  start  of  a  new  frame.  It  spends  exactly  one  frame 
period  at  state  01  performing  image  detection  then  moves  into  state  11. 
It  will  wait  here  until  COMPLETE  becomes  high,  denoting  that  both  de¬ 
coders  have  reached  state  11  of  their  own.  It  then  enters  state  10 
during  which  START  will  be  cleared.  Finally,  it  goes  back  to  state  00 
and  waits  here  for  START  to  go  high  to  begin  another  cycle. 

The  above  state  diagram  was  realized  in  Fig.  3.17  by  using  four 
74112  J-K  flip-flops  and  a  number  of  gates.  The  clock  signal  which 
triggers  the  transition  from  one  state  to  another  is  the  EF  (Even  Field) 
signal  from  CCU.  An  EF  signal  always  advances  a  new  frame,  thus 
conveying  the  vital  timing  information. 

The  START  signal,  which  initiates  an  image  detecting  cycle,  can  be 
set  either  manually  through  a  switch  SI  on  the  interface  board  or  under 
the  control  of  the  11/70,  which  uses  bit  RD1  to  trigger  the  operation 
when  switch  S4  is  set  to  a  '1'  (scanning  mode)  and  both  decoders  are  at 
state  '00'.  Signal  R01  is  the  least  significant  bit  of  the  byte  of  data 
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sent  from  the  11/70  to  the  vision  system;  detailed  description  about 
scanning  control  by  the  computer  is  given  in  Section  3.6.  It  is  cleared 
by  the  negative  pulse  OS'  used  to  strobe  the  UAR/T,  by  an  external  reset 
switch  S2,  or  upon  POI.  The  implementation  is  shown  in  Fig.  3.18. 

The  COMPLETE  signal  is  set  when  both  decoders  arrive  at  state  11  of 
their  own.  It  is  cleared  as  decoder  1  goes  back  to  state  00.  The  im¬ 
plementation  is  given  in  Fig.  3.19. 

3. 3. 6. 3  Image  Coordinate  Decoder 

There  is  one  image  coordinate  decoder  associated  with  each  camera. 
It  receives  intensity  data  of  individual  pixels  from  CCU,  searches 
through  an  image  frame  and  computes  the  X  and  Y  coordinates  of  the  first 
bright  image  whose  intensity  level  is  above  an  external  threshold.  The 
image  coordinate  system  is  shown  in  Fig.  3.20.  There  are  244  rows  and 
248  columns  in  a  frame.  Thus  it  requires  8  bits  to  represent  X  and  Y 
coordinates  respectively.  The  search  is  done  on  a  row  by  row  basis; 
i.e.,  the  first  row  is  scanned,  followed  by  the  second  row,  then  the 
third  row  and  so  on.  The  block  diagram  of  an  image  coordinate  decoder 
is  shown  in  Fig.  3.21.  It  consists  of  an  X  coordinate  decoder,  a  Y 
coordinate  decoder,  and  a  comparator.  Both  decoders  keep  on  counting 
until  a  pixel  whose  intensity  level  is  higher  than  the  threshold  is  en¬ 
countered.  At  this  point,  the  decoders  stop  counting  and  their  values 
are  preserved.  A  flag  is  set  to  indicate  that  a  bright  spot  is 
detected.  If  no  bright  image  is  found  at  the  end  of  a  frame,  the  flag 
remains  cleared  and  the  content  of  the  X  decoder  is  0  and  that  of  the  Y 
decoder  is  either  239  or  240.  The  image  cordinates  (XI, Yl)  and  (X2,Y2) 
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Figure  3.20  The  Image  Coordinate  System. 


Figure  3.21  Block  Diagram  of  an  Image  Decoder. 
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are  then  multiplexed  and  fed  to  the  UAR/T.  The  control  logic  block  is 
part  of  the  interface  controller  described  last  section. 

The  8-bit  X  coordinate  decoder  is  cleared  at  the  beginning  of  each 
row  by  the  SBLNK  signal,  and  is  incremented  by  one  for  every  pixel  clock 
ERC.  It  was  implemented  by  cascading  two  74LS161  binary  counters,  as 
shown  in  Fig.  3.22.  The  contents  of  these  counters  are  zero  after  POI 
or  RESET.  Furthermore,  at  a  given  instant  the  decoder  is  in  one  of  the 
four  states  as  explained  earlier,  and  image  detection  occurs  only  in 
state  01.  During  the  course  of  image  detection  within  a  frame,  these 
counters  are  cleared  at  the  very  beginning  of  each  row,  which  is  initi¬ 
ated  by  SBLNK.  On  the  other  hand,  if  an  image  is  detected  the  values  of 
the  counters  should  be  preserved.  Therefore  SBLNK  is  AND'ed  with  Q1',Q0 
and  IM 1 .  States  Q1‘  and  QO  are  presented  to  ensure  that  the  decoder  is 
indeed  in  the  image  detecting  state.  Signal  IM'  is  a  flag  indicating  if 
an  image  has  been  detected  so  far.  If  no  image  is  encountered  ,  IM'  is 
high  and  the  searching  and  clearing  continues.  If  an  image  is  detected, 
IM'  becomes  low  and  the  clearing  signal  is  disabled  so  the  contents  of 
the  counters  are  stored.  The  ERC  signal  is  fed  to  the  CK  pin  of  both 

counters  so  that  it  can  count  up  the  number  of  pixels  in  a  row.  The  IM1 

and  the  state  variables  Q1 *  and  QO  are  AND'ed  and  fed  to  the  ENT  (count 
enable)  and  ENP  (carry  look-ahead  enable)  pins  of  the  lower  counter  so 
that  it  counts  up  only  before  an  image  is  sensed.  The  ENT  and  ENP  pins 
of  the  high  nibble  counter  is  connected  to  the  CARRY  pin  of  the  low 
nibble  one  so  that  it  can  count  up  to  255. 

The  8-bit  Y  coordinate  decoder,  shown  in  Fig.  3.23,  differs  from 

the  X  decoder  in  two  aspects.  First,  it  is  cleared  only  once  at  the 

beginning  of  a  searching  frame,  a  condition  realized  by  the  AND'ed 
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Figure  3.23  Y-Coordinate  Decoder. 


function  of  VSYNC,  Q1 ‘  and  QQ.  Second,  it  is  incremented  by  rows 
instead  of  pixels.  Hence,  the  CK  pin  of  the  counters  are  connected  to 
the  S8LNK/2  signal.  This  signal  is  obtained  by  feeding  SBLNK  through  a 
74109  J-K  flip-flop.  It  is  necessary  to  divide  the  SBLNK  signal  by  2 
because  each  row  is  scanned  and  displayed  twice  by  CCU  to  accommodate 
the  standard  525  line  TV  frame.  The  third  bit  of  the  Y  counter  is  used 
to  set  the  CROW  signal  for  the  purpose  of  skipping  the  first  four  rows 
because  they  do  not  contain  useful  information. 

The  8-bit  comparator  is  implemented  by  using  two  7485' s.  The 
physical  arrangement  is  shown  in  Fig.  3.24.  Eight  input  pins,  AO  to  A7, 
are  connected  to  a  dip  switch  which  provides  the  external  threshold. 
Another  eight  pins,  BO  to  B7,  are  tied  to  the  output  of  an  8-bit  latch, 
74273,  which  furnishes  the  intensity  of  the  pixel  currently  being 
sensed.  The  A<3  pin  of  the  comparator  is  used  to  clear  the  no  image 
flag  IM1.  The  input  of  the  latch  is  the  3-bit  digital  video  signal 
coming  from  the  CCU.  It  is  very  important  to  latch  the  intensity  data 
at  the  right  time  so  the  CK  pin  is  controlled  by  the  AND 'ed  signal  of 
ERC '  ,Q1‘ ,Q0,IM*  and  CROW.  The  CROW  signal  is  generated  by  the  Y  decoder 
to  skip  the  first  four  rows  as  mentioned  earlier. 

3. 3. 6.4  UAR/T  Controller 

At  the  completion  of  an  image  detecting  cycle,  five  bytes  of  data 
are  generated.  They  are  X1,Y1,X2,Y2,  and  a  status  byte.  The  organi¬ 
zation  of  the  status  byte  is  shown  in  Fig.  3.25.  A  '1'  in  bit  IM1  indi¬ 
cates  that  an  image  has  been  detected  by  decoder  1,  otherwise,  a  'O' 
will  be  stored  there.  Bit  IM2  carries  similar  information  for  decoder 
2.  A  ' 1 *  in  bit  SCAN  denotes  that  scanning  operation  under  11/70 
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Figure  3.24  The  Intensity  Comparator. 


control  is  in  effect,  while  a  '0'  means  that  manual  operation  mode  is 
adopted.  A  '1‘  in  bit  CONTINUE  directs  the  11/70  to  keep  on  scanning 
and  a  'O'  stop  it.  A  ‘ 1 *  in  bit  READY  informs  the  11/70  that  the  image 
currently  being  sensed  is  a  desired  foothold  and  the  triangul ation 
routine  should  be  called  to  compute  the  three  dimensional  coordinate. 

These  five  bytes  of  data  must  be  sent  to  the  11/70  computer  for 
further  processing.  Technically,  this  can  be  done  either  in  parallel  or 
in  serial.  Parallel  transmission  is  fast  but  more  expensive  because 
first,  a  DEC  parallel  interface  card  would  be  needed  and  second,  a  large 
cable  is  required  due  to  the  fact  that  every  line  connected  to  the  11/70 
has  to  be  optically  isolated,  which  requires  two  wires  for  each  line. 
Serial  transmission  is  slower;  nevertheless,  the  data  rate  of  this 
application  is  low  enough  to  be  well  handled  serially.  Therefore  the 
serial  scheme  was  adopted. 

A  UAR/T  is  used  to  facilitate  data  transmission  and  receiving.  A 
sequential  circuit,  the  UAR/T  controller,  is  designed  to  oversee  the 
function  of  the  UAR/T  and  ensure  that  exactly  5  bytes  are  transmitted  at 
the  right  time.  The  UAR/T  chip  used  is  a  General  Instrument  Corporation 
AY-5-1012  product.  It  is  a  40  pin  LSI  unit  which  accepts  binary  char¬ 
acters  from  either  a  terminal  device  or  a  computer  and  recei ves/trans- 
mi ts  this  character  with  appended  control  and  error  detecting  bits.  All 
characters  contain  a  start  bit,  5  to  8  data  bits,  one  or  two  stop  bits, 
and  either  odd/even  parity  or  no  parity.  The  baud  rate,  parity  mode, 
and  the  number  of  stop  bits  are  externally  selectable.  A  diagram  show¬ 
ing  the  pin  configuration  is  given  in  Fig.  3.26. 

The  trananission  of  data  to  the  11/70  computer  is  regulated  by  a 
clocked  sequential  circuit,  the  state  diagram  of  which  is  shown  in  Fig. 
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Figure  3,25  Organization  of  the  STATUS  Byte. 
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Figure  3.26  Pin  Configuration  of  the  UAR/T  Chip. 
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3.27.  Initially,  it  is  idle  at  state  A.  The  beginning  of  transmission 
is  activated  by  the  TRANSMIT  signal,  which  is  set  to  high  in  the  sai..e 
way  as  the  COMPLETE  signal  (see  Section  3. 3. 6. 2),  which  signifies  the 
end  of  the  image  detecting  phase.  When  TRANSMIT  becomes  high,  it  enters 
state  B.  It  stays  at  B  for  one  clock  cycle,  clears  all  registers  inside 
the  UAR/T  oy  pulling  the  XR  (External  Reset)  pin  high.  Its  activity  in 
the  following  five  states,  C,D,E,F,and  G  are  all  similar.  Upon  entering 
each  of  these  states,  a  negative  pulse  is  generated  and  fed  into  the  DS 
(Data  Strobe)  pin  to  have  one  byte  of  data  loaded  into  the  transmitter 
buffer  of  the  UAR/T  and  then  transmitted,  while  the  controller  waits 
there.  When  a  byte  is  completely  sent  out,  the  T3MT  (Transmitter  Buffer 
Empty  Flag)  is  set  to  indicate  that  the  UAR/T  is  ready  to  accept  another 
byte,  and  the  controller  moves  into  the  next  state.  This  action  is  re¬ 
peated  five  times  and  exactly  five  bytes  of  data  are  transmitted  one 
after  one.  The  TRANSMIT  signal  is  cleared  during  state  H  and  the  con¬ 
troller  then  goes  back  to  state  A.  The  flip-flop  input  equations  can  be 
easily  derived.  They  are: 


J2=gl.Q0'.TBMT, 

(3.10) 

K2=Ql‘.g0‘ , 

(3.11* 

Ji=g2'.gu, 

(3.12) 

Kl=g2.gO.TSMT, 

(3.13) 

jg=g2.gi.TBMT+Q2'.gl‘.  TRANSMIT, 

(3.14) 

K0=g2.Ql'+TbMT.g2,.Ql 

(3.15) 

These  equations  were  implemented  by  using  three  74112  negative-edge 
triggered  J-K  flip-flops.  The  circuit  diagram  is  shown  in  Fig.  3.28. 
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Figure  3.27  State  Diagram  of  the  Transmitter  Controller. 
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The  clock  signal  which  provides  the  timing  base  for  the  controller 
as  well  as  the  transmitter  and  receiver  of  the  UAR/T  is  generated  from  a 
555  timer,  as  shown  in  Fig.  3.29.  The  clock  frequency  can  be  adjusted 
in  a  range  from  3.5  KHz  to  100  KHz.  The  output  of  the  555  timer  is  fed 
to  a  7414  Schmitt  trigger  to  steepen  the  leading  and  trailing  edges. 

The  negative  data  strobe  required  to  load  data  into  UAR/T  is  pro¬ 
duced  by  a  74121  monostable  mul ti vibrator  as  shown  in  Fig.  3.30.  The 
manufacturer  requires  that  the  data  strobe  be  at  least  250  nsec,  in 
length.  The  resistor  and  capacitor  chosen  give  a  duration  of  700  nsec. 
It  is  crucial  that  one  and  only  one  pulse  be  generated  in  each  of  the 
five  states  involved;  therefore  a  7474  flip-flop  is  dedicated  to  each 
state.  Note  that  the  data  strobe  is  fed  back  to  clear  each  7474;  other¬ 
wise,  the  output  of  7430  would  remain  high  once  an  involved  state  is 
travsersed  and  consequently  only  one  pulse,  instead  of  five,  will  be 
generated. 

The  five  bytes  of  data  are  multiplexed  and  then  fed  to  the  UAR/T. 

It  is  arranged  such  that  the  ST  (STatus  byte)  is  loaded  first  during 
state  C,  followed  by  XI,  Yl,  X2,  and  Y2.  This  portion  of  the  circuit  is 
shown  in  Fig.  3.31. 

The  part  of  controller  which  guards  the  receiving  function  is  much 
simpler.  There  are  only  two  states  involved,  and  its  state  diagram  and 
implementation  are  shown  in  Fig.  3.32.  It  remains  idle  in  state  0  until 
a  byte  has  been  received  and  assembled.  This  event  is  signaled  by 
UAR/T1 s  pulling  the  DA  (Data  Available)  line  high.  When  this  occurs,  it 
moves  into  state  1  where  the  received  byte  is  latched  onto  an  8-bit 
register  (74273).  After  one  clock  cycle,  it  resets  the  data  available 
flag  and  returns  to  state  0.  Although  one  byte  of  data  is  received  from 
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Figure  3.32  The  Receiver  Controller. 

(a)  Implementation;  (b)  State  Diagram. 


the  11/70  each  time,  only  two  bits  carry  information.  A  ‘O'  to  *1' 
transition  in  RD1  triggers  an  image  detecting  operation  (recall  Fig. 
3.18),  and  a  '1'  in  R02  clears  the  READY  flag  in  the  status  byte.  The 
READY  bit  is  set  by  the  operator  through  a  remote-controlled  switch  to 
indicate  that  a  foothold  has  been  selected. 

The  PQI  (Power-On  Initialization)  signal  was  obtained  by  feeding  a 
+5V  source  through  a  IK  resistor  and  a  30  ^F  capacitor,  as  shown  in  Fig. 
3.33.  The  external  RESET  signal  was  implemented  using  a  debounced 
toggle  switch  as  shown  in  Fig.  3.34. 

3. 3. 6. 5  Optical  Isolation 

To  protect  the  11/70  computer  from  damages  resulting  from  possible 
abnormal  function  of  an  external  circuit,  the  serial  communication 
channel  is  optically  isolated  as  shown  in  Fig.  3.35.  Data  from  the  SO 
(Serial  Output)  pin  of  the  UAR/T  is  fed  through  a  MC1468P  line  driver  to 
meet  the  RS-232  standard,  which  demands  voltage  levels  of  ±  15V  instead 
of  the  ±  5V  TTL  level.  The  output  of  the  line  driver  is  connected  to  a 
HP2630  optocoupler.  In  a  short  circuit  situation,  the  diode  in  the 
input  side  of  HP2630  will  be  burnt  out  but  the  other  side  of  circuit 
will  not  be  affected  because  there  is  no  physical  connection  between 
them.  Under  normal  conditions,  the  input  current  passing  through  the 
diode  will  activate  the  other  side  of  circuit.  Since  the  output  of  a 
HP2630  is  TTl  compatible,  another  line  driver  is  needed  to  convert  it 
into  RS-232  level.  Data  sent  out  from  the  11/70  is  optically  isolated 
as  well,  and  is  received  at  the  SI  (Serial  Input)  pin. 
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Figure  3.33 


The  Power-On  Initialization. 


Figure  3.3 4  The  External  RESET . 
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3. 3. 6. 6  The  Interface  Circuit  Board 

The  interface  circuit  was  constructed  on  an  AUGAT  IIL-I  circuit 
ooard.  Its  layout  is  shown  in  Fig.  3.36,  and  a  picture  of  the  the 
physical  board  is  given  in  Fig.  3.37.  The  signals  from  CCU1  are  con¬ 
nected  to  connector  J1  of  group  A  through  flat  cable  Cl.  Similarly, 
signals  from  CCU2  are  fed  to  connector  J1  of  group  B  by  using  C2. 
Connector  J1  of  group  C  is  tied  to  the  TT3  terminal  cor.nector  through 
C3,and  the  ±I2V  from  the  power  supply  unit  are  connected  to  J1  of  group 
0.  The  image  coordinates  XI  and  Y1  are  made  available  at  J2  of  group  A, 
and  so  are  X2  and  Y2  at  J2  of  group  B.  Port  J1  of  group  E  is  connected 
to  a  remote,  push-button  switch  which  is  used  by  an  operator  to  set  the 
READY  flag  of  the  status  byte  when  laser  beam  has  been  focused  on  a  de¬ 
sired  spot. 

There  are  five  switches  on  board.  Switch  SI  is  used  to  trigger  the 
image  detecting  operation  in  the  manual  control  mode.  A  positive  trans¬ 
ition  of  SI  would  cause  the  circuit  to  search  for  an  image  once.  Switch 
S2  is  the  external  reset  switch.  When  S2  is  high,  both  image  decoders 
and  the  interface  controller  are  reset  to  their  initial  ststes.  Switch 
S4  indicates  whether  it  is  a  manual  or  a  scanning  operation.  With  S4 
down,  the  operation  is  controlled  manually  through  SI.  With  S4  and  S5 
both  up,  the  circuit  would  continue  scanning  under  computer  control. 

Pull  S5  down  would  terminate  the  scanning.  Switch  S3  is  not  used. 

There  are  four  dip  switches  denoted  as  UI0,  1123,  U30  and  1)43 
respectively .  Among  them,  U10  and  1)23  are  associated  with  camera  1  and 
the  other  two  with  camera  2.  Switch  UI0  is  used  to  control  Gl,  G2,  IIG 
and  I/244SEQ  pins  of  camera  1  so  that  it  is  operated  in  a  sequential 
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mode  with  injection  inhibit  disabled.  Pins  G1  and  G2  are  both  tied  low 
so  that  all  the  clock  and  digital  output  signals  are  always  available. 
Switch  U23  is  the  external  threshold  used  to  detect  a  bright  image.  The 
functions  of  U30  and  U43  are  exactly  the  same  as  those  of  U10  and  1123. 

The  variable  resistor  pot  PI  is  used  to  adjust  the  clock  frequency 
of  the  UAR/T  to  accommodate  a  chosen  baud  rate. 

3.4  Human  Interaction 

The  responsibility  of  the  hunan  operator  is  to  select  proper  foot¬ 
holds  for  the  Hexapod.  To  accomplish  this  function  reliably,  a  communi¬ 
cation  channel  is  needed  so  that  the  operator  can  tell  the  vision  sys¬ 
tem,  which  is  searching  for  an  image  point  constantly,  whether  this  is 
the  image  of  a  desired  foothold.  One  way  to  handle  this  problem  is  to 
impose  a  "hand-shaking"  policy,  which  requires  the  operator  to  set  a 
READY  flag  for  a  selected  point  and  has  the  vision  system  clear  it  after 
the  image  is  processed.  Physically,  a  remote-control  led,  push-button 
switch  connected  to  the  interface  board  through  a  long  cable  is  held  by 
the  operator.  When  the  switch  is  pushed,  its  output  signal  is  first 
debounced  then  fed  to  a  flip-flop  (74112)  whose  output  is  the  READY  bit 
of  the  status  byte.  This  part  of  circuitry  was  given  in  Fig.  3.38. 

The  11/70  computer,  after  computing  the  coordinates  of  that  foothold, 
immediately  sends  out  one  byte  of  data  and  uses  the  second  bit  (RD2)  to 
clear  the  READY  flag. 

3.5  Operational  and  Testing  Procedures  of  The  Vision  System 

To  insure  that  the  vision  system  functions  properly,  the  procedures 
listed  below  should  be  followed. 
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(1)  Select  a  baud  rate  and  adjust  the  clock  frequency 


accordingly.  A  selectable  range  of  baud  rate  from  0  to  9600 
is  available  [50].  For  this  application,  a  baud  rate  higher 
than  600  is  sufficient.  After  choosing  a  particular  rate, 
one  should  adjust  pot  PI  on  the  interface  board  and  measure 
the  clock  frequency  by  an  oscilloscope  or  a  frequency  meter. 
Note  that  the  tolerable  clock  skew  of  DH-11  is  4.8%,  thus 
the  frequency  of  the  transmitter  and  receiver  of  the  UAR/T 
should  be  accurate  to  that  extent.  The  system  has  been 
operated  at  a  baud  rate  of  1200  with  highly  stable  results. 

(2)  Set  up  equipment.  One  end  of  cable  Cl  is  connected  to  the 
interface  connector  of  CCU1  and  the  other  end  to  the  J1 
connector  of  group  A  on  the  interface  board.  Cable  C2  should 
be  connected  to  the  similar  location  of  CCU2  and  J1  of  group 
8.  Cable  C3  ought  to  be  connected  between  J1  of  group  C  and 
the  TT3  terminal  connector,  and  the  power  lines  at  J1  of 
group  D  are  tied  to  the  +12V  of  the  power  supply  unit.  The 
remote-controlled  switch  must  be  connected  to  the  J1  port  of 
group  E. 

(3)  Set  the  apertures  of  both  lenses  to  be  minimum  (f/16).  With 
this  aperture,  a  proper  threshold  for  image  intensity  is  12, 
which  is  high  enough  to  supress  internal  noise  and  low  enough 
to  be  overriden  by  the  desired  image. 


(4)  Enter  the  following  commands  at  the  system  console: 

>set  /speed=tt3: 1200: 1200  (assime  that  baud  rate  is  1200), 

>set  /slave=tt3: 

>set  /fdx=tt3: 

The  speed  command  notifies  the  11/70  computer  that  the  vision  ter¬ 
minal  which  was  assigned  as  TT3  is  operating  at  a  baud  rate  of  1200. 

The  slave  command  causes  the  vision  terminal  to  be  treated  as  a  slave 
while  tne  11/70  is  the  master.  A  slave  terminal  will  not  send  any 
message  toward  tne  computer  unless  it  is  asked  by  the  master  to  do  so. 
With  this  arrangement,  any  unsolicited  data  generated  by  noise  will  be 
labelled  as  such  and  the  computer  is  able  to  differentiate  image  data 
provided  by  tne  vision  terminal  from  noise.  The  fdx  command  establishes 
the  communication  link  between  TT3  and  the  11/70  as  a  full  duplex 
channel . 

Enter  the  following  command  from  the  terminal  that  one  logs 
i  n: 

>AS,N  TT3:  =QT : 

This  command  informs  the  computer  that  TT3  is  the  input 
as  well  as  the  output  terminal  associated  with  the  I/O 
requests  appearing  in  the  program. 

(6)  Set  up  the  laser  head  and  laser  power  supply. 

(7)  Turn  on  the  power  supply  and  both  CCU's. 

(8)  Pull  switch  S2  up  and  down  to  reset  the  vision  system.  Set 
switches  S4  and  S5  in  the  'up'  pos:tion  to  put  it  in  the 
scanning  mode. 
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(d)  To  test  if  the  vision  system  functions  normally,  run  program 
V I □.  If  unsolicited  data  exits,  the  program  will  abort  itself 
and  VID  has  to  be  run  again.  Otherwise,  the  message  "SPECIFY 
FOOTHOLD"  is  displayed  on  the  screen  to  prompt  the  operator 
for  a  foothold.  The  laser  unit  should  be  turned  on  and  the 
beam  be  aimed  to  a  point  which  is  within  the  field  of  view  of 
both  cameras;  then  the  remote  switch  is  pushed.  If  the  image 
is  sensed  by  both  cameras,  the  X,  Y,  and  Z  coordinates  will  be 
displayed  on  screen,  or  else  the  character  string  "NO  IMAGE, 

TRY  AGAIN"  is  shown  and  the  operator  has  to  adjust  the  laser 
head  and  then  activates  the  remote  switch  again. 

3.6  Scanning  Control  Of  The  Vision  System 

The  software  which  deals  with  the  interaction  between  the  11/70  and 
the  interface  circuit  and  enables  the  computer  to  control  the  scanning 
of  the  vision  system  deserves  special  explanation  because  (1)  it  in¬ 
volves  activities  embedded  at  a  lower  level  and  requires  assembly 
language  programming,  which  is  not  as  easy  to  comprehend  as  codes 
written  in  PASCAL,  and  (2)  it  utilizes  many  of  the  RSX-11  system  direc¬ 
tives.  Although  detailed  documentation  of  them  are  provided  by  [51],  a 
brief  description  of  each  directive  whenever  it  is  encountered  would 
make  it  easier  to  follow.  Therefore,  the  detail  of  program  VID,  which 
manages  the  interaction  of  the  11/70  and  the  vision  system  to  facilitate 
scanning  control,  is  examined  here.  Its  block  diagram  is  shown  in  Fig. 
3.39. 

First  of  all,  the  vision  terminal  has  to  be  logically  connected  to 
the  11/70  system  so  that  the  operating  system  can  keep  track  on  its 
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Figure  3.39  Flowchart  of  Proaram  VI D. 
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status.  This  is  done  through  the  fallowing  statements: 

LUN  =  9. 

ALUN$$  #LUN,#"0T,#Q 

QI0W5S  #I0.ATT,#LUN 

The  variaole  LUN  represents  the  particular  logical  unit  number  that  the 
vision  terminal  is  assigned.  The  execution  of  directive  ALUNSS  in¬ 
structs  the  system  to  assign  a  physical  device  unit  to  a  logical  unit 
number.  Thus,  the  physical  device  whose  name  is  "OT"  is  assigned  a 
logical  unit  number  of  LUN,  which  has  a  value  of  9.  Recall  that  the 
vision  terminal  is  TT3,  hence  if  the  command  [ASN  TT3:=0T:]  is  issued 
from  a  VT100  terminal  console,  the  vision  terminal  will  has  a  logical 
unit  number  of  9.  The  next  instruction,  QI0W5S,  instructs  the  system  to 
place  an  I/O  request  for  the  device  with  the  logical  unit  number  LUN  and 
wait  until  the  request  is  fulfilled.  The  subcommand  #10. ATT  specifies 
that  this  particular  I/O  task  is  to  attach  this  device  to  the  system. 
After  it  is  executed,  the  communication  link  between  the  11/70  and  the 
vision  terminal  is  logically  connected  and  completely  established. 

The  next  step  is  to  check  whether  unsolicited  data  exists.  Every 
I/O  device  is  allocated  an  input  and  an  output  buffer  by  the  operating 
system.  (The  directions  of  input  and  output  are  referred  to  with  res¬ 
pect  to  the  11/70.)  The  input  buffer  of  a  slave  device  should  be  empty 
if  no  input  request  has  been  issued  by  the  computer.  The  only  occasion 
that  the  11/70  would  ask  the  vision  system  to  send  in  data  is  during 
foothold  acquisition.  Thus,  before  it  initiates  such  an  event,  any  data 
in  the  input  buffer  is  interpreted  as  unsolicited  data.  In  other  words, 
the  number  of  bytes  of  data  in  the  input  buffer  must  be  zero  prior  to  an 
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input  request.  This  quantity  may  be  accessed  by  using  the  following 
di recti ve: 

Q10WSS  fSF.GMC.fLUN, fEFl , , ,  ,  <#STAD, #2> 

Again  this  is  an  I/O  request  instruction  because  it  involves  the  vision 
terminal.  The  subcommand  fSF.GMC  directs  the  system  to  return  the  in¬ 
formation  of  device  #LUN.  Particul arly ,  if  the  variable  STAD  is  initi¬ 
alized  to  be  TC.TBF  by  using  the  BYTE  directive,  the  returned  value 
gives  the  number  of  bytes  of  data  in  its  input  buffer.  If  this  value  is 
not  zero,  then  unsolicited  data  exists  and  the  program  will  abort 
i  tsel f . 

To  initiate  an  image  detecting  event,  the  computer  sends  out  one 
byte  of  data  to  the  vision  terminal  and  uses  the  least  significant  bit 
to  trigger  the  interface  circuit.  This  output  operation  is  achieved  by 
the  fol lowi ng  code: 

QlUWSb  TlO.WAL.fLUN, fEFl, ,#I0ST, ,<tfWRBUF ,fl> 

The  subcommand  -IO.WAL  orders  the  system  to  write  (or  output)  data  to 
device  fLUN.  Moreover,  parameter  <#WRSUf  ,fl>  indicates  that  only  one 
byte  is  to  be  sent  out  and  that  this  byte  of  data  is  located  at  WRBUF. 

The  process  of  the  program  will  be  halted  until  the  output  event  is 
completed  as  signaled  by  the  event  flag  EF1.  The  status  of  this  request 
is  made  accessible  at  location  IOST. 

While  the  vision  system  is  searching  for  an  image,  the  computer 
prepares  for  the  arrival  of  5  bytes  of  data  as  the  result  of  image 
searching  by  declaring  another  I/O  request: 
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QI0W5S  *IG.RAL,#LUN,#EF1, ,*I0ST,,<#RDBUF,#5> 

This  command  instructs  the  system  to  read  5  bytes  of  data  from  device 
*LUN  and  stores  them  at  location  RDBUF.  The  subcommand  #I0.RAL  simply 
indicates  that  this  is  an  input  request.  The  meanings  of  EF1  and  IOST 
are  the  same  as  described  earlier. 

Among  the  5  bytes  of  received  data,  the  status  byte  is  investigated 
first.  Above  all,  the  READY  bit  is  checked  to  see  if  this  is  the  image 
of  a  specified  foothold.  If  not,  nothing  will  be  done  except  examining 
the  CONTINUE  flag  to  decide  whether  the  scanning  operation  is  to  be  con¬ 
tinued.  In  case  that  the  READY  bit  is  set,  which  implies  this  image  is 
of  interest,  Itol  and  IM2  are  checked  to  determine  whether  the  image  is 
detected  oy  Doth  cameras.  If  either  one  or  both  miss  it,  a  message  is 
displayed  on  a  screen  to  prompt  the  operator  to  give  another  try.  In 
addition,  the  11/7U  will  send  out  one  byte  of  data  and  use  the  second 
bit  (R02)  to  clear  the  READY  flag.  If  both  IM1  and  IM2  are  set,  tri¬ 
angulation  is  performed  to  compute  the  three-dimensional  coordinates  of 
the  detected  image.  This  subject  is  discussed  in  depth  in  the  next 
chapter. 

3. 7  Summary 

In  this  chapter,  problems  encountered  in  realizing  FTL  locomotion 
were  identified  and  possible  solutions  were  explored.  After  comparisons 
of  different  al ternatives,  the  approach  which  employs  a  binocular  vision 
system  was  deemed  more  suitable  than  others  and  thereby  was  adopted.  To 
control  the  front  feet  of  the  Hexapod,  an  operator  points  a  laser  beam 
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at  terrain  surface  points,  which  are  then  perceived  by  the  vision  system 
as  the  footholds. 

The  problem  of  selecting  camera  mounting  locations  was  analyzed  in 
consideration  of  the  field  of  view  and  mechanical  stability.  It  was 
concluded  that  5.5  mm  wide-angle  lenses  were  needed  in  order  to  satisfy 
the  requirements. 

Both  narrow-band  and  broad-band  optical  filters  have  been  tested  to 
study  their  noise  rejection  capability.  The  former  indeed  blocked  out 
most  of  the  ambient  noise  but  also  severely  attenuated  the  returned 
laser  signal,  while  the  latter  demonstrated  an  inferior  noise  rejection 
capability.  It  was  found  that,  inside  a  laboratory  and  under  normal 
lighting  condition,  the  most  efficient  way  to  reject  ambient  light  while 
maintaining  sufficient  intensity  for  reflected  laser  beam  is  to  reduce 
the  aperture  of  the  lens  to  the  minimum  value  of  f/16  with  no  filters  on 
it.  This  may  be  due  to  the  nonlinearity  of  the  internal  electronic  cir¬ 
cuits  of  the  camera  heads.  Extermely  strong  light  sources  should  be 
avoided  since  they  will  not  be  totally  rejected  even  with  a  minimum 
aperture. 

A  special-purpose  hardware  circuit  was  designed  and  constructed  to 
compute  the  image  coordinates  of  an  object  point  in  both  camera  frames 
and  to  send  the  data  to  the  11/70  computer.  Details  of  the  design  and 
implementation  were  presented. 

The  vision  system  is  treated  by  the  11/70  as  a  regular  peripheral 
terminal.  Their  communication  is  based  on  a  master-and-sl ave,  hand¬ 
shaking  scheme.  The  software  which  enables  the  11/70  to  control  the 
scanning  operation  of  the  vision  system  has. also  been  described. 
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Chapter  4 


CALIBRATION  OF  THE  VISION  SYSTEM 


4.1  Introduction 

The  determination  of  the  spatial  coordinates  (X,Y,Z)  of  a  foothold 
takes  two  steps.  First,  the  image  coordinates  in  both  camera  frames  are 
obtained  through  the  interface  circuit.  Then,  a  projection  ray  is  iden¬ 
tified  for  the  image  point  on  each  frame  and  the  intersection  of  these 
two  rays  yields  the  location  of  the  foothold,  a  process  which  is  known 
as  triangul ation.  The  accuracy  of  the  result  depends  on  how  precise 
these  projection  rays  are  defined.  This  requires  a  proper  camera  model, 
an  accurate  measurement  of  the  internal  geometric  parameters  of  the 
camera,  and  compensation  to  correct  for  lens  distortion.  Furthermore, 
the  coordinates  computed  have  to  be  transformed  from  the  camera  CS 
(Coordinate  System)  to  the  Hexapod  body  CS,  which  calls  for  the  identi¬ 
fication  of  the  relation  between  the  camera  and  the  body  CS's.  These 
subjects  are  discussed  in  this  chapter.  A  pinhole  camera  model  is 
introduced  in  Section  4.2.  Based  on  this  model,  the  mapping  between  a 
3-0  object  point  and  its  corresponding  2-D  image  point  is  formulated. 

In  Section  4.3,  procedures  for  camera  calibration  are  described.  In 
Section  4.4,  distortion  caused  by  a  wide-angle  lens  is  characterized 
by  a  polynomial  and  compensated  accordingly.  In  Section  4.5,  the 
triangul ation  technique  is  described.  The  arrangement  of  camera 
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installation  by  using  two  ball-socket  heads  and  its  effect  on  the  common 
field  of  view  are  presented  in  Section  4.6.  The  geometrical  relation¬ 
ships  between  cameras  and  the  Hexapod  body  are  identified  in  Section 
4.7,  with  cameras  being  attached  to  the  mounting  frame  by  using  fixtures 
instead  of  ball-socket  heads. 

4.2  The  Camera  Model 

The  purpose  of  an  analytic  camera  model  is  to  relate  3-D  coordi¬ 
nates  of  points  in  object  space  to  the  2-D  locations  of  their  images  in 
a  digitized  picture  frame.  An  adequate  and  widely  used  model  is  the  so- 
called  pinhole  model  in  which  the  camera  is  represented  by  a  pinhole 
lens  together  with  an  image  plane  lying  at  distance  of  F  behind  the 
lens,  where  F  is  the  focal  length  of  the  lens.  The  model  adopted  here 
is  very  similar  to  a  pinhole  model  except  that  the  image  plane  is  placed 
in  front  of  the  lens  center  to  avoid  the  inconvenience  of  inverted 
images.  Fig.  4.1  shows  such  a  camera  model  with  a  coordinate  system 
assigned  to  it.  The  origin  of  this  camera  CS  is  chosen  at  the  upper- 
left  corner  so  that  the  image  coordinates  agree  with  those  that  are 
generated  by  the  image  coordinate  decoders.  Mote  that  the  Z  coordinate 
is  0  for  every  point  on  the  image  plane.  The  coordinates  of  the  lens 
center  in  this  coordinate  system  are  denoted  as  (Xc,Yc,-F). 

Based  on  this  model,  the  relationship  between  an  object  point  and 
its  image  can  be  readily  established.  Given  any  point  { X , Y , Z )  in  the 
object  space,  its  image  (Xp.Yp)  can  be  uniquely  determined;  and  knowing 
an  image  (Xp.Yp),  one  can  identify  the  corresponding  projection  ray. 
Referring  to  Fig.  4.1  and  assuming  that  the  3-D  coordinates  of  object 
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point  0  are  (Xo,Yo,Zo),  the  straight  line  passing  through  0  and  the  lens 
center  C  can  be  characterized  by  the  following  equation: 

Xo-X  _  Yo-Y  _  Zo-Z 

X-Xc  =  Y-Yc  =  Z+F  (4,1) 

In  this  equation,  (X,Y,Z)  are  the  coordinates  of  an  arbitrary  point 
lying  on  the  segment  CO.  Obviously,  the  image  point  (Xp,Yp,Zp)  is  one 
of  such  points.  Thus 

Xo_-Xp  -  Yq-Yp  . 

Xp-Xc  Yp-Yc  Zp+F 

Recall  that  the  Z  coordinate  of  any  point  on  the  image  plane  is  0, 
i .  e . ; 


Zp=0 


(4.3) 


Substituting  Eq.  (4.3)  into  (4.2)  results  in 

Xo-Xp  Zo  Yo-Yp 
Xp-Xc  F  Yp-Yc 


Therefo re 

( Z0/F )  .  Xc+X0 
XP  =  l+( Zo/F) 

„  (Zo/F)  .  Yc+Yo 

Yp  =  - 

l+( Zo/F ) 


(4.5) 


(4.6) 
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Since  the  picture  is  digitized,  the  image  coordinates  IX  and  IY  are 
integers  wi th 


IX  =  R0D(Xp/0x) 

(4.7) 

IY  =  R0D(Yp/0y) 

(4.8) 

Here  ROD  is  the  round-off  function,  Dx  is  the  length  of  a  pixel  in  the  X 
direction  and  Dy  is  the  length  in  the  Y  direction. 

On  the  other  hand,  if  an  image  (IX, IY)  is  detected  and  it  is  de¬ 
sired  to  find  out  the  projection  ray,  one  may  proceed  as  follows. 
Assuming  that  the  image  is  located  at  the  center  of  the  pixel  (IX, IY), 
then  its  coordinates  are 

Xp  =  Dx- ( IX-1/2 )  (4.9) 

Yp  =  Dy. (IY-1/2)  (4.10) 

Zp  =  0  (4.11) 

The  projection  ray  is  expressed  as  follows: 

X-Xp  _  Y-Yp  _  Z 

Xp-Xc  Yp-Yc  F  *  (4.12) 


which  can  be  written  into  a  set  of  parametric  equations 

(4.13) 

(4.14) 

(4.15) 


X  =  Xp+K. (Xp-Xc) 
Y  =  Yp+K. (Yp-Yc) 
Z  =  K.F 


with  K  as  the  parameter. 
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4.3  Camera  Calibration 


To  accurately  locate  a  point  in  space  by  using  vision,  the  relation 
between  a  point  on  the  image  plane  and  the  projection  ray  associated 
with  this  point  should  be  known  precisely.  This  relation  was  esta¬ 
blished  in  the  last  section  by  using  a  pinhole  model  with  a  front  image 
plane.  During  the  derivation,  the  coordinates  of  the  lens  center 
(Xc,Yc,-F)  were  assumed  to  be  known.  From  Eqs.  (4.5),  (4.6),  (4.13), 
(4.14)  and  (4.15)  it  is  clear  the  mappings  between  images  and  object 
points  are  functions  of  these  parameters.  Hence,  it  is  important  to 
precisely  determine  the  location  of  the  lens  center  as  represented  in 
the  camera  CS.  These  internal  geometric  parameters  are  calibrated  in 
this  section. 

The  calibration  is  done  in  two  steps.  First,  the  optical  axis 
of  the  camera  is  aligned  to  a  specific  direction  and  second,  several 
object  points  with  known  coordinates  are  shown  to  the  camera  to  find 
out  their  image  coordinates.  With  these  data,  Xc,  Yc  and  F  can  be 
computed. 

A  wooden  calibration  frame  was  constructed  and  used  to  align  the 
optical  axis.  This  frame  has  three  axes  which  are  perpendicul ar  to  one 
another  just  like  a  Cartesian  CS.  Five  tiny  light  sources  were  attached 
at  different  locations  as  shown  in  Fig.  4.2.  To  align  the  optical  axis 
of  the  camera  to  the  Z  axis  of  the  frame,  the  camera  is  placed  in  front 
of  the  calibration  frame  and  all  light  sources  are  lit  up.  The  location 
and  orientation  of  the  camera  is  then  gradually  adjusted  and  the  pattern 
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Figure  4.2  The  Calibration  Frame. 


5 

2 

(a) 

Figure  4. 


5 


3  Image  Patterns  for  Aligning  the  Optical 
Axis  of  a  Camera. 

(a)  and  (b)  are  Out  of  Alignment;  (c) 
is  In  Alignment. 


of  the  image  is  observed  on  a  monitor.  If  the  camera  is  out  of  align¬ 
ment,  five  images  will  be  seen  on  the  screen  as  shown  in  Fig.  4. 3. a  and 

4.3. b.  If  the  camera  is  aligned,  only  four  images  are  observed  because 
the  images  of  light  sources  1  and  2  are  overlapped,  as  shown  in  Fig. 

4.3. c.  A  surface  level  was  attached  on  top  of  each  camera  as  another 
means  to  ensure  the  pitch  angle  is  zero  during  calibration. 

After  alignment  is  achieved,  the  coordinates  (Xi,Yi,Zi)  of  each 
light  source  are  measured  manually.  Then  the  image  coordinates 
(IXi.IYi)  are  obtained  from  the  interface  circuit.  Thus,  a  5-tuple 
( X i , Y i , Zi , I X i , I Y i )  is  generated  for  each  light  source.  From  IXi  and 
IYi,  the  coordinates  of  the  image  point  in  camera  CS  can  be  found  as: 


Xpi 

=  Dx. ( IXi -1/2 ) 

(4.16) 

Ypi 

=  l)y*  ( IYi  -1/2) 

(4.17) 

Zpi 

=  0 

(4.18) 

where  Dx=46  ^m  and  0y=36  ^m. 

For  each  light  source,  two  equations  in  the  form  of  Eqs.  (4.5)  and 
(4.6)  are  obtained.  They  are 

(Zi/F).Xc+Xi 

XP’  =  i+(Zi/F)  (4.19) 

Ypi  =  ( Zi  /F )»  Yc+Y i 

l+(Zi/F)  (4.20) 
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Rearranging  Eqs.  (4.19)  and  (4.20)  yields 


Zi.Xc+F. (Xi-Xpi)  =  Zi.Xpi  (4.21) 

Zi.Yc+F. (Yi-Ypi)  =  Zi - Yp i  (4.22) 

It  is  clear  that  with  the  data  of  any  two  light  sources  (except  1 
and  2  since  they  lie  on  the  same  line),  four  equations  in  the  form  of 
Eqs.  (4.21)  and  (4.22)  may  be  derived  and  the  three  unknowns  Xc,  Yc  and 
F  can  be  solved.  A  program,  CENTER,  has  been  written  to  take  data  from 
two  light  sources  and  return  the  values  of  Xc,  Yc  and  F. 

Each  of  the  two  cameras  was  calibrated  separately,  and  they  did 
exhibit  different  values  of  Xc  and  Yc.  One  important  observation  was 
that  the  image  of  the  optical  axis,  a  point  where  the  optical  axis 
intersects  the  image  plane,  does  not  coincide  with  the  center  of  the 
image  sensor.  The  image  sensor  has  244  rows  and  248  columns  hence  the 
image  coordinates  of  the  center  of  the  image  sensor  should  be  (124,122). 
However,  it  was  found  that  the  optical  axis  of  camera  1  intersects  the 
image  plane  at  (119,118).  For  camera  2  ,  that  location  is  at  (111, 

113). 


4.4  Lens  Calibration 

The  existence  of  image  distortion  can  be  verified  in  at  least  two 
ways.  In  one  case,  a  picture  containing  the  junction  line  of  a  wall  and 
the  floor  was  displayed  on  a  monitor.  Physically  it  is  a  straight  line, 
but  when  observed  on  the  screen,  it  became  a  curve,  as  shown  in  Fig. 

4.4.  In  another  test,  eight  light  sources  were  placed  in  a  horizontal 
line  with  the  distance  between  any  two  neighbors  being  equal.  Then  the 


108 


109 


1 

] 

Image  coordinates  of  each  light  were  taken  and  compared.  It  was  found 
that  the  difference  in  image  coordinate  of  two  neighbors  was  smaller  at 
both  ends. 

To  deal  with  the  distortion  caused  by  the  wide-angle  lenses,  one 
has  to  know  the  type  of  distortion,  and  whether  it  can  be  expressed 
analytically  and  corrected. 

Two  types  of  distortion,  barrel  distortion  and  pincushion  dis¬ 
tortion  [52],  are  the  most  common  distortions  that  may  appear  in  an 
optical  system.  Their  patterns  of  distortion  are  shown  in  Fig.  4.5. 

Comparing  Fig.  4.4  with  Fig.  4.5,  it  is  apparent  that  the  distortion 
exists  in  cameras  used  in  this  research  is  not  of  the  pincushion  type 
but  closer  to  the  barrel  distortion. 

For  barrel  distortion,  the  amount  of  distortion  is  proportional  to 
the  cube  of  the  radius  of  an  image  [52].  The  radius  of  an  image  is 
defined  as  the  distance  between  the  image  and  the  image  of  the  optical 
axis.  Although  the  observed  distortion  may  not  be  a  pure  barrel  dis¬ 
tortion,  it  is  of  similar  nature.  Thus  it  was  considered  reasonable  to 
express  the  distortion  aR  as  a  polynomial  function  of  radius  R,  i.e.; 

AR  =  KnR  +  Kn_i  R  +  ....  +  K^R  +  K0  (4.23) 

To  collect  distortion  data,  square-ruled  papers  were  posted  on  a 
blackboard.  A  light  source  was  manually  moved  from  one  location  to 
another  location.  At  each  position,  a  5-tuple  (Xi ,Yi ,Zi , I Xi ,IYi )  was 
obtained  as  mentioned  earlier.  Using  Eqs.  (4.19)  and  (4.20),  one  can 
find  the  distortionless  image  (Xpi.Ypi).  The  distance  between  this 
point  and  the  image  of  the  optical  axis  is  the  non-di storted  radius  Ri . 


no 


Lens  center 


Figure  4,6  Relationships  Between  the  Nondistorted  Image 
the  Distorted  Image  b,  and  the  Image  of  the 
Optical  Axis  c. 


The  image  detected  by  the  interface  circuit,  (IXi.IYi),  is  a  distorted 
image.  Its  radius  is  denoted  as  IRi .  The  amount  of  di stortion  ARi  is 
the  difference  of  Ri  and  IRi.  This  is  as  illustrated  in  Fig.  4.6. 

ARi  =  Ri -IRi  (4.24) 

Thus,  a  pair  (aRi  ,Ri )  is  generated  from  each  5-tuple.  Program  CAC1 
(listed  in  Appendix)  reads  a  5-tuple  data  from  an  external  file,  com¬ 
putes  (A.Ri.Ri),  and  stores  them  into  another  data  file. 

The  next  step  is  to  fit  collected  data  with  a  polynomial  in  the 
form  of  Eq.  (4.23).  Two  questions  arise  here  with  respect  to  the  degree 
of  the  polynomial  and  the  determination  of  the  coefficients.  The 
approach  taken  was  to  begin  with  the  lowest  degree  possible  (a  first 
order  polynomial),  and  increase  the  degree  by  one  each  time,  meanwhile 

comparing  the  results  of  the  curve  fitting.  The  performance  of  an  mth 
* 

order  polynomial  is  defined  as 

Sm  =  |  IaRi  -  Pm(Ri)i  (4.25) 

i  =1 

where  Pm  is  the  mth  order  polynomial,  n  is  the  number  of  data  points, 
and  Sm  is  the  performance  index  of  Pm. 

A  program  POLYFT  has  been  created  for  the  above  purpose.  Given  the 
desired  order  m  of  the  polynomial  and  the  number  of  data  points,  it  re¬ 
turns  the  value  of  5m  and  all  the  coefficients.  The  coefficients  are 
calculated  by  an  algorithm  based  on  a  set  of  orthogonal  polynomials 
[53],  and  the  polynomial  thus  obtained  is,  among  all  mth  degree  poly¬ 
nomials,  the  one  that  best  fits  the  data  in  the  minimum-square-error 
sense.  If  Sm  is  significantly  smaller  than  Sm-1,  then  polynomials  of 

112 


i 


degree  m+1  should  be  tried  and  so  on.  Otherwise,  Pm-1  would  be  chosen 
«...  che  desired  polynomial. 

After  going  through  the  process  just  described,  it  was  concluded 
that  a  4th  order  polynomial  should  be  used.  The  coefficients  for  camera 
1  are 


—  “ 

-  • 

A4 

-6.42E5 

A3 

7.8779E3 

A2 

= 

1.6978E1 

A1 

1. 11458E-1 

AO 

1. 17956E-4 

-  m 

(4.26) 


Those  for  camera  2  are 


- 

— 

B4 

-6.432E5 

B3 

8.9594E3 

82 

= 

3.9178 

B1 

-6. 7241E-2 

_B°_ 

8.38079E-5 

(4.27) 


Given  these  coefficients,  lens  calibration  can  be  stated  as 
follows:  given  an  distorted  image( IX, IY) ,  find  the  undistorted  radius  R 
which  may  in  turn  be  used  to  locate  the  distortionless  image.  From 
(IX, IY),  the  quantity  IR  is  computed.  Combining  Eq.  (4.23)  and  (4.24) 
results  in 


AR  =  R-IR  =  K4R4  +  K3  R3  +  K2R2  +  K^R2  +  Kc 


(4.28) 
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Rearranging  yields 


K4R4  +  K3R3  +  K2R2  +  {K 1 -1 ) R  +  K0  =  IR  (4.29) 

Therefore,  by  solving  the  real  roots  of  a  4th  order  polynomial,  the 
value  of  R  is  obtained.  Since  images  are  inwardly  distorted,  the  un¬ 
distorted  radius  R  must  be  greater  than  the  distorted  radius  IR.  This 
condition  can  be  used  to  choose  the  correct  root  in  cases  where  there 
are  more  than  one  real,  positive  roots.  A  program  R00T4  (see  Appendix) 
was  written  for  this  purpose  using  the  Newton-Raphson  method. 

The  lens  calibration  data  are  listed  in  Table  4.1  and  Table  4.2. 
Here  IXD  and  I  YD  are  the  distorted  image  coordinates  furnished  by  the 
hardware  circuit,  IXUD  and  IYUD  are  the  undistorted  image  coordinates 
computed  using  the  camera  model,  and  IXC  and  IYC  are  the  compensated 
values  obtained  by  using  the  lens  calibration  procedure  described  above. 
Table  4.1  is  associated  with  Camera  1  and  Table  4.2  is  for  Camera  2. 
Rote  that  without  correction,  the  distortion  may  be  as  large  as  10 
pixels,  and  after  compensation,  it  is  reduced  to  within  1  pixel. 

In  the  above  analysis,  distortion  aR  is  expressed  as  a  polynomial 
function  of  the  undistorted  image  radius  R.  It  may  also  be  possible  to 
represent  aR  as  a  polynomial  function  of  the  distorted  image  radius  IR. 
Then  adding  aR  to  IR  would  give  the  value  of  R.  This  approach  is  com¬ 
putationally  more  efficient  because  it  is  not  necessary  to  solve  for  the 
real  roots  of  a  fourth  order  polynomial. 
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TABLE  4.1 

Lens  Calibration  Data  of  Camera  1 


4 


1 


(IXD  and  IYD  are  distorted  image  coordinates; 
IXUD  and  I YUO  are  the  undistorted  coordinates; 
and  IXC  and  IYC  are  the  compensated  values.) 


10 

168 

85 

167 

84 

168 

84 

11 

136 

51 

135 

51 

135 

50 

12 

136 

68 

135 

68 

135 

68 

13 

136 

84 

135 

84 

135 

84 

14 

103 

83 

102 

84 

102 

85 

15  76 


16  49 


17  47 


18  19 


19  15 

20  13 


TABLE  4.2 


Lens  Calibration  Data  of  Camera  2. 

(IXD  and  IYD  are  distorted  image  coordinates; 
IXUD  and  IYUD  are  the  undistorted  coordinates; 
and  IXC  and  IYC  are  the  compensated  values.) 


IXD 

IYD 

IXUD 

IYUD 

IXC 

IYC 

1 

195 

15 

210 

-3 

210 

-4 

2 

196 

28 

210 

14 

209 

14 

3 

218 

33 

242 

14 

242 

14 

4 

220 

45 

242 

30 

243 

30 

5 

221 

58 

242 

46 

243 

46 

6 

223 

99 

242 

96 

243 

96 

7 

223 

113 

242 

112 

242 

113 

8 

172 

23 

177 

14 

178 

13 

9 

176 

97 

177 

96 

178 

96 

10 

143 

20 

144 

14 

144 

13 

11 

112 

47 

111 

46 

111 

46 

12 

54 

15 

46 

5 

46 

3 

13 

50 

72 

46 

71 

46 

70 

14 

83 

8 

79 

-3 

79 

-2 

15 

68 

8 

62 

-3 

62 

-4 

I 

1 
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4.5  Triangul ation 


It  is  clear  that  every  object  point  has  a  single,  well  defined 
image  point.  Thus,  given  an  object  point  0,  the  corresponding  image 
point  V  on  the  image  plane  is  uniquely  defined.  The  inverse  mapping, 
however,  is  not  one-to-one.  For  each  image  point  there  is  a  line  in 
space,  defined  by  the  image  point  and  the  lens  center,  along  which  the 
corresponding  object  point  must  lie.  Additional  information  is  needed 
to  determine  the  exact  location  of  the  object  point.  The  standard 
method  for  this  purpose  is  called  stereoscopy  [54],  and  is  based  upon 
the  use  of  two  pictures. 

4.5.1  Mathematical  Model  Or  Triangul ation 

For  stereoscopic  analysis,  a  binocular  system  can  be  represented  as 
in  Fig.  4.7.  Here,  0  is  the  object  point  to  be  located,  II  and  12  are 
the  image  points  on  the  two  image  planes,  Cl  and  C2  are  the  lens  centers 
and  D  is  the  displacenent  vector  between  Cl  and  C2.  Knowing  II  and 
Cl,  a  vector  R1  is  determined.  Similarly,  another  vector  R2  is 
defined  by  C2  and  12.  The  intersection  of  R1  and  R2  gives  the  lo¬ 
cation  of  0.  Mathematical ly ,  this  can  be  written  as 

rI  =  D  +  R2  (4.30) 

Let  dl  and  1)2  be  the  unit  vectors  along  Rl  and  R2,  Eq.  (4.30)  becomes 
auT  =  0  +  bU2"  (4.31) 
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where  a  and  b  are  constants. 

Ideally,  R1  and  R2  should  intersect  at  a  unique  point.  However, 
due  to  various  errors,  the  two  projection  rays  will  in  general  fail  to 
intersect.  A  reasonable  solution,  proposed  by  Duda  and  Hart  [54],  is  to 
place  0  midway  between  Rl  and  R2  where  the  distance  between  them  is 
minimum,  i.e., 

0  =  [a0uT  +  (D+b0U2)]/2  (4.32) 


where  a0  and  b0  are  the  values  of  a  and  b  that  minimize 


J  { a ,  b )  = 


aUl  -  (D+bU2 ) 


To  solve  for  a0  and  b0,  rewrite  J(a,b)  as 
J  ( a , b )  =[auT-(D+buF)]»  [aUl"-(D+bU2)] 


(4.33) 


(4.34) 


Expanding  Eq.  (4.34)  yields 


J(a,b)=a2  -  2aUi«D  -'2abUi.U2  +  2bU2  .  D  +  D2  +  b2  (4.35) 


The  conditions  for  minimizing  J ( a , b )  are 
dJ/da=0 

dJ/db=U  (4.36) 


which  lead  to 


0 


(4.37) 


2a  -  2U i  •  (0  +  bU2 )  = 

2b  -  2112  *  (°  “  aUl^  =  0  (4.38) 


Substituting  Eq.  (4.37)  into  Eq.  (4.38)  and  eliminating 

a  gives 

the  variable 

b  +  1)2  •  D  -  (U1.U2)(U1.D  +  bl)]_»  U2)  =  0 

(4.39) 

Solving  for  b  results  in 

(u7-u7) (u7 D )  -  (uF*D) 

D0  “ 

1  -  (Ui- u2)2 

(4.40) 

Similarly, 

( U  i«  D )  -  (UtU?)OI?.D) 

ao  ~ 

1  -  (Ui  .  u2)2 

(4.41) 

Finally,  the  position  of  0  is 

0  =  u  ^ 

(4.42) 

or 

0  =  0  +  b0»U2 

(4.43) 

4.5.2  Physical  Considerations 

In  the  above  analysis,  all  positions  and  vectors  are  represented  in 
coordinate-free  notations.  For  the  actual  arrangement  of  a  binocular 
system,  two  CS's  are  involved,  one  for  each  camera.  Information  such  as 
image  coordinates  (II  and  12)  and  locations  of  lens  centers  (Cl  and  C2) 
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are  expressed  in  two  different  CS's.  Before  applying  the  results  from 
4.5.1,  it  is  necessary  to  define  a  common  CS  and  refer  everything  to 
it.  Here,  camera  1  CS  is  chosen  as  the  reference  CS  (camera  2  CS  can  be 
used  as  well).  Furthermore,  the  geometric  relation  between  the  two 
cameras  must  be  known  so  that  entities  in  the  camera  2  CS  can  be  trans¬ 
formed  to  the  camera  1  CS.  This  relation  may  be  represented  as  a  HTM 
(Homogeneous  Transformation  Matrix)  [55].  A  HTM  which  describes  the 
spatial  relation  of  two  arbitrary  CS's  A  and  8  i s  a  4  by  4  matrix  de¬ 
noted  as  bT  ,  and  the  elements  of  this  matrix  are  defined  as  follows: 

cl 


til 

1 12 

1 1 3 

tl4 

t21 

1 22 

t23 

t24 

t31 

t32 

t33 

t34 

0 

0 

0 

1 

[_0  0  0  1J  (4.44) 

where  ( tl4,t24,t34)  is  the  representation  of  the  origin  of  the  A  CS  in 
the  B  CS.  The  column  (tli,t21,t31)  is  the  representation  of  the  unit  X 
vector  of  the  A  CS  in  the  B  CS.  Similarly,  (tl2,t22,t32)  and 
( tl3,t23,t33)  are  the  representations  of  the  unit  Y  vector  and  the  unit 
Z  vector  of  the  A  CS  in  the  B  CS.  The  9  elements  ( tll,tl2, . . . ,t33) 
specify  the  "orientation"  of  the  A  CS  with  respect  to  the  B  CS,  while 
the  3  elements  ( tl4,t24,t34)  contain  the  "displacement"  information. 

For  example,  bTa  for  CS's  A  and  B  in  Fig.  4.8  is 


T  =  1 

a 


0  -2 


(4.45) 


1 

J 


By  means  of  this  HTM,  the  coordinates  of  any  point  (Xa,Ya,Za)  in  the  a 
CS  can  be  transformed  into  the  representation  in  the  b  CS  (Xb,Yb,Zb)  as 


Xb 

Xa 

Yb 

=  bT  . 

Ya 

Zb 

a 

Za 

1 

—  m 

1 

An  important  character!' stic  of  a  HMT  bT^  is  that 

\  =  (bT  f1  (4.47) 

D  a 

Suppose  that  the  image  coordinates  of  11  are  (1X1,  IY1).  Then  its 
3-D  coordinates  as  expressed  in  camera  1  CS  are  (X1,Y1,Z1)  where 


Xl=Dx.  (IX1-1/2), 

(4.48) 

Yl=0y. (IY1-1/2), 

(4.49) 

Z1«0 

(4.50) 

coordinates  of  the  lens  center  Cl  are  (XC1 ,YC1, ZC1 ) . 

vector  Rl,  (RXl.RYl.RZl) ,  are  simply 

The  components 

RX1=X1-XC1, 

(4.51) 

RY1=Y1-YC1, 

(4.52) 

RZ1=Z1-ZC1 . 

(4.53) 

Similarly,  the  coordinates  of  12  and  C2  are  (X2,Y2,Z2)  and  (XC2.YC2, 
ZC2),  respectively,  as  expressed  in  the  camera  2  CS.  These  entities 
are  transformed  to  camera  1  CS  by  using  HTM  *T 


,  i -e. ; 


12*  *  LT2  .  12  , 

(4.54) 

C2 1  =  lJ2  .  C2 

(4.55) 

R2  =  12'  -  C2' . 

(4.56) 

The  unit  vectors  U1  and  U2  are  then  computed  as  follows 

uT  =  rT/|ri|  , 

U2=R2/|R2|.  (4.57) 

The  displacement  vector  0  is  the  same  as  the  4th  column  vector  of  *T^ 
but  without  the  last  element  1.  That  is, 

OX  =1T2(4,1) , 

Oy  =  1T2(4,2) 

JZ  =  lT  (4,3).  (4.58) 

Now  with  all  vectors  computed  and  expressed  in  the  camera  1  CS, 
one  can  substitute  them  into  Eqs.  (4.40)  and  (4.41)  and  find  the 
location  of  0.  The  described  triangul ation  is  done  by  the  program 
TRIANG  which  takes  ,  1X1, I Yl, 1X2  and  IY2  as  input  data  and 
returns  the  coordinates  of  0  in  camera  1  CS. 

4.6  Camera  Installation  Arrangement 

The  cameras  are  installed  onto  the  Hexapod  by  using  a  n  -shaped 
frame,  designed  and  built  by  the  Mechnical  Engineering  Department,  and 
two  KALT  #5020  ball -socket  heads  (Fig.  4.9).  A  drawing  showing  the 
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Figure  4 


1 

frame  in  relation  to  the  Hexapod  is  given  in  Fig.  4.10.  A  picture 
showing  the  physical  mounting  is  given  in  Fig.  4.11.  The  cameras  are 
mounted  directly  to  the  ball-socket  heads,  which  are  then  attached  to 
the  frame  through  two  mechanical  adaptors  (G  and  H).  The  frame  is  in¬ 
stalled  onto  the  Hexapod  by  screwing  the  bases  BJ  and  OK  tightly  to  the 
existing  structure  so  that  it  is  rigid  and  stable.  Adaptors  G  and  H  can 
move  along  segment  £F,  thus  the  separation  between  the  cameras  is  ad¬ 
justable.  Segment  £F  can  slide  up  and  down  along  segments  AB  and  CD, 
hence  the  altitude  of  both  cameras  is  subject  to  change.  These  provide 
sufficient  flexbility  in  choosing  suitable  locations  for  the  cameras. 

Moreover,  the  ball-socket  head  allows  a  camera  to  be  tilted  and  rolled, 
and  therefore  adds  two  more  degrees  of  freedom. 

4. b. 1  Altitude  Of  Cameras 

With  a  6.5  mm  wide-angle  lens,  a  camera  can  posess  a  2  m  wide 
field  of  view  at  an  altitude  of  1.2  m  above  ground.  Although  the 
structure  allows  a  maximum  height  of  1.5  m  and  a  larger  working  dis¬ 
tance  would  provide  wider  field  of  view,  the  received  intensity  of  an 
image  becomes  weaker  as  a  camera  is  installed  higher  thus  the  image  is 
more  likely  to  be  skipped  by  the  image  decoder.  Also,  lower  camera 
mounting  would  give  better  mechanical  stability.  Hence,  it  is  advan¬ 
tageous  to  keep  the  altitude  of  cameras  lower  so  long  as  the  field  of 
view  is  large  enough  to  cover  the  area  of  interest. 

4.6.2  Separation  Between  Cameras 

The  distance  between  the  cameras  will  affect  both  the  CFOV  (Common 
Field  Uf  View)  and  the  accuracy  of  triangulation.  A  large  separation  is 
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Figure  4.10  Camera  Mounting  Frame  and  the  Body 
of  the  Hexapod. 
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desirable  so  that  triangulation  gives  accurate  result  [37].  A  smaller 
separation,  however,  permits  better  CFOV.  A  compromise  value,  40  cm, 
was  picked  after  a  number  of  experiments. 

4.6.3  Tilt  And  Roll  Angles 

A  judicious  selection  of  a  tilt  angle  and  a  roll  angle  will  produce 
a  better  CFOV.  The  following  analysis  will  verify  this  point.  It  in¬ 
volves  a  series  of  coordinate  transformation  from  the  camera  CS  to  the 
bo4y  CS.  Two  intermediate  CS's  E  and  F  are  introduced  to  facilitate  the 
explanation.  Fig.  4.12  shows  a  case  in  which  both  tilt  and  roll  angles 
are  zero.  The  sizes  of  the  camera  and  the  ball-socket  are  exaggerated 
to  make  the  picture  clear.  The  origin  of  the  E  CS  corresponds  to  the 
junction  point  of  the  moveable  segment  and  the  cone-shaped  base.  The 
X  axis  is  assigned  to  be  along  the  direction  of  the  segment,  the  Z  axis 
is  perpendicul ar  to  the  ground,  and  the  Y  axis  is  determined  by  the 
right-hand  rule.  The  origin  of  the  F  CS  is  at  the  center  of  the  base  of 
the  ball-socket  head.  The  X  axis  is  perpendicul ar  to  the  base,  the  Z 
axis  is  perpendicul  ar  to  the  ground,  and  again  the  Y  axis  follows  the 
right-hand  rule.  The  camera  CS,  denoted  as  CA,  is  as  described  in 
Section  4.2. 

Regardless  of  the  tilt  and  roll  angles,  the  CA  and  E  CS’s  are 
related  by  a  HTM 

1  0  lx 

0  0  ly 

0  1  lz 

0  0  1  (4.59) 
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To  tilt  the  camera  for  an  angle  of  e,  the  segment  is  rotated  around 
the  Y  axis  for  an  angle  of  e.  At  this  position,  the  E  and  F  OS's  are 
related  by  a  second  HTM 


cose  0 

0  1 

-sine  0 

0  0 


si  ne  px 

0  0 

cose  0 

o  1 


(4.60) 


Then,  if  the  camera  is  rolled  by  an  angle  of  $  by  rotating  the  base 
about  its  X  axis,  the  F  and  body  CS's  can  be  related  by  a  third  HTM 


1 

0 

0 

dx 

0 

COS<t, 

-sin} 

dy 

0 

sirvj) 

cos* 

dz 

0 

0 

0 

1 

(4.61) 


Multiplying  these  matrices,  the  relation  between  the  CA  and  body 
CS's  is  given  by 


bt  =  bt  •  ft 

CA  F  E 


CA 


tca" 


0  -cose  sine 
cos<t  -sinesinj)  -sirv^cose 
sirvj>  sinecos<t>  cos^cose 
0  0  0 


lxcose  +lzsine+dx+px 
1  xsi  nj)  sine  +lycos<j)  -lzsi  rv^cose  +dy 
-1  xsi  ne  cos<j) +ly  si  hj, +1  zcose  cos<d +1  z 
1 

(4.62) 
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Since  the  goal  is  to  find  the  field  of  view  as  seen  from  the  body 
CS,  it  is  appropriate  to  explain  here  how  a  field  of  view  is  determined. 

i 

A  field  of  view  is  formed  by  the  four  intersection  points  of  four  vec¬ 
tors  with  ground.  These  four  vectors  all  originate  from  the  lens  center 
but  pass  through  a  different  corner  of  the  image  sensor,  as  shown  in 
Fig.  4.13.  Knowing  the  3-D  coordinates  of  the  lens  center  and  the  four 
corners  in  CA  CS,  these  vectors  can  be  computed.  By  using  HTM  T,  these 
vectors  can  be  expressed  in  the  body  CS.  The  ground,  when  represented 
in  the  body  CS,  is  a  plane  characterized  by  an  equation  Z=k,  with  k 
depending  on  the  altitude  of  the  body.  By  solving  for  the  intersections 
of  four  lines  and  one  plane,  four  points  are  obtained,  and  the  trapezoid 
defined  by  these  points  is  the  field  of  view.  The  following  example 
illustrates  the  concept  outlined  above  by  finding  the  point  of  inter¬ 
section  of  the  vector  C4  and  the  ground.  The  other  three  vectors  can  be 
treated  in  exactly  the  same  way. 

The  coordinates  of  the  lens  center  are  (Xc,Yc,Zc),  and  those  of 
corner  4  are  (X4,Y4,Z4)  with 

X4=  248. Ox, 

Y4=244.Dy, 

Z4=Q  (4.63) 

These  coordinates  are  transformed  into  the  bo<fy  CS  by 


- 1 

*<  X 

-p  p- 

_ " _ Li 

-  8t  . 

1 

X  V- 

L _ 

r 

O  <J 

-  bt  . 

1 

-<  X 

o  o 
- 1 

Z4* 

1 

CA 

Z4 

1 

- 

9 

V 

1 

CA 

Zc 

1 

(4.64) 
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The  line  that  passes  through  C  and  corner  4  can  be  described  by  the 
following  parametric  equations: 

X=X4‘ +m(X4‘ -Xc1 ) 

Y=Y4'+m(Y4'-Yc‘ ) 

Z=Z4,+m(Z4'-Zcl )  (4.65) 

Recall  that  the  ground  is  a  plane  with  Z=k,  which  means  that  for  the 
intersection  point 

k=Z4,+m(Z4'-Zc‘ )  (4.66) 

thus  the  unknown  m=(k-Z4‘ )/ ( Z4 1 -Zc 1 )  and  the  value  of  X,Y  and  Z  are 
now  computed. 

A  program,  FOVF,  is  available  which  generates  the  coordinates  of 
the  four  vertices  of  the  field  of  view  for  given  tilt  and  roll  angles. 
The  resultant  field  of  view  is  displayed  on  the  HP  graphic  terminal  so 
that  one  can  easily  see  if  it  covers  the  area  of  interest.  Fig.  4.14 
through  Fig.  4.17  are  typical  results  generated  by  FOVF.  Both  tilt 
and  roll  angles  are  zero  for  the  case  of  Fig.  4.14,  which  means  that 
both  cameras  look  straight  down  to  the  ground.  Mote  that  leg  1  is 
barely  covered  by  the  camera  on  the  right-hand  side,  thus  it  may  miss  a 
foothold  that  is  to  the  left  of  leg  1.  Also  note  that  the  front  (and 
rear)  edges  of  the  two  field  of  views  fail  to  overlap  each  other.  This 
is  due  to  the  non-ideal  location  of  the  lens  center  with  respect  to  the 
focal  plane,  as  discussed  in  Section  4.3.  Fig.  4.15  is  the  result 
produced  by  a  tilt  angle  of  20  degrees  and  a  roll  angle  of  0  degree. 
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Figure  4.14  CFOV  with  a  Tilt  Angle  of  0  degree  and  a 
Roll  Angle  of  0  degree. 


Figure  4.15  CFOV  with  a  Tilt  Angle  of  20  degrees  and  a 
Roll  Angle  of  0  degree. 
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Figure  4.16  CFOV  with  a  Tilt  Angle  of  0  degree 
and  a  Roll  Angle  of  20  degrees. 


Figure  4.17  CFOV  with  a  Tilt  Angle  of  20  degrees 
and  a  Roll  Angle  of  5  degrees. 
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The  field  of  view  of  both  cameras  are  tilted  forward,  and  the  coverage 
of  the  area  of  interest  does  not  improve.  For  the  case  of  a  pure  roll 
angle,  as  shown  in  Fig.  4.16  where  the  tilt  angle  is  zero  and  the  roll 
angle  is  20  degrees,  the  result  is  not  satisfactory  either  in  terms  of 
the  coverage  of  the  area  of  interest.  With  a  judicious  choice  of  tilt 
and  roll  angles,  however,  the  CFOV  can  be  greatly  improved  as  seen  in 
Fig.  4.17,  where  the  tilt  angle  is  20  degrees  and  the  roll  angle  is  5 
degrees. 

4.7  Geometrical  Relationships  Between  Cameras  And  The  Hexapod  Body 

After  some  experiments,  it  was  observed  that  the  ball -socket  head 
was  not  rigid  enough  to  lock  itself  to  the  selected  pitch  and  roll 
angles.  Thus  the  geometrical  relationship  between  the  cameras  and  the 
body  could  dr’^t  away  from  what  it  should  be,  and  an  error  is  introduced 
as  the  coordinates  of  a  foothold  are  transformed  from  the  camera  CS  to 
the  body  CS.  To  eliminate  this  problem,  the  ball -socket  head  was  re¬ 
placed  by  fixtures  which  allow  the  cameras  to  be  more  tightly  attached 
to  the  mounting  frame.  Fig.  4.18  shows  the  top  view  of  this  arrange¬ 
ment.  Fig.  4. 19. a  shows  that  Camera  1  being  attached  to  the  right  fix¬ 
ture,  and  Fig.  4.19.b  shows  the  physical  connection  between  Camera  2  and 
the  left  fixture.  With  the  new  fixtures,  the  relationships  between 
cameras  and  body  are  different  from  those  when  ball -socket  heads  were 
used  and  must  be  reidentified. 

The  geometrical  relation  between  two  CS'  s  can  be  decomposed  into 
two  parts:  position  and  orientation.  In  an  HTM,  orientation  is  repre¬ 
sented  by  the  upper-left  3x3  matrix  and  position  is  treated  as  a 
vector  in  the  rightmost  column.  Relating  to  the  physical  setup, 
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Left  Right 

Fixture  Fixture 


Figure  4.18  Top  View  of  the  Cameras,  Fixtures,  and 
the  Horizontal  Bar. 
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Pictures  Showing  Cameras  Being  Attached  to 
Fixtures,  (a)  Camera  1  and  the  Right  Fixture; 
(b)  Camera  2  and  the  Left  Fixture. 


the  position  component  relates  to  where  a  fixture  is  placed  on  the 
frane.  Changes  in  position  can  be  made  by  sliding  a  fixture  up  and 
down,  in  and  out.  Since  the  fixtures  were  cut  in  a  special  way,  there 
is  only  one  possible  orientation  once  a  camera  is  fastened  onto  it. 

This  unique  orientation  may  be  identified  by  examining  the  structure  of 
the  fixture.  Fig.  4.20  shows  the  top  and  two  side  views  of  the  right 
fixture.  The  left  fixture  is  a  mirror  reflection  of  the  right  one. 

To  help  the  explanation,  another  CS,  called  the  F  (Fixture)  CS,  is 
introduced  and  has  its  origin  at  point  1  as  shown  in  Fig.  4.20.  Several 
observations  which  lead  to  the  determination  of  the  orientation  of  a 
camera  with  respect  to  the  right  fixture  are  as  follows: 

(1)  the  X  axis  of  camera  CS  coincides  with  vector  42, 

(2)  the  XZ  plane  of  camera  CS  lies  on  the  plane  passing  through 
points  1,2  and  3,  (thus  the  negative  Y  axis  of  camera  CS  has  the 
same  direction  as  the  normal  vector  of  plane  123),  and 

(3)  the  Z  axis  is  completely  specified  by  the  right  hand  rule  once 
the  X  and  Y  axes  are  located. 

The  actual  computation  is  as  follows.  First,  the  coordinates  of 
points  1,2,3  and  4  are  found  to  be  (0,0,0),  (1.1,  0,  3),  (-0.45,  -5, 

0),  and  (0.6,  -5,  3)  respectively  as  expressed  in  the  F  CS.  Vectors 
formed  by  these  points  are  computed  as: 

Vl2  -  (1.1,  0,  3) 

V2 3  =  (-0.45,  -5,  0) 

T42  =  (0.5,  5,  0)  (4.67) 


140 


8y  observation  (1),  the  representation  of  the  X  axis  of  camera  CS  in 
the  F  CS  is  the  same  as  V42.  Thus 

X  =  (0.5,  5,  0)  (4.68) 


The  cross  product  of  V12  and  V23  yields  the  normal  vector  of  the  plane 
123,  which  may  be  computed  by  evaluting  the  determinant  of  the  following 
3x3  matrix: 


V 12  *  ^23 


x  y  z 

1.1  0  3 

-0.45  -5  0 


(4.69) 


The  result  is  N  =  (15,  -1.35,  -5.5).  After  normalization,  the  normal 
vector  N  becomes  (0.9355,  -0.0842,  -0.343).  By  observation  (2),  The  Y 
axis  of  camera  CS  is  represented  as  (-0.9355,  0.0842,  0.343).  The  Z 
axis  is  then  computed  as  the  cross  product  of  X  and  Y  as  follows: 


X  x  Y 


x  y 

0.0995  0.995 

-0.9355  0.0842 


z 

0 

0.343; 


which  gives  Z  =  (0.341,  -0.0341,  0.9392). 


(4.70) 


With  X,  Y  and  Z  computed  as  above,  the  orientation  portion  of 
B 

T  is  completely  specified  as  shown  below: 

CA1 


0.0995 

-0.9355 

0.341 

px 

0.995 

0.0842 

-0.0341 

py 

(4.71) 

0 

0.343 

0.9392 

pz 

0 

0 

0 

1 
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The  position  component  (px.py.pz),  which  represents  the  coordinates  of 
the  origin  of  camera  CS  as  seen  from  body  CS,  can  usually  be  obtained  by 
manual  measuranents.  For  instance,  the  position  vector  of  camera  1  for 
the  present  installation  is  (-0.011,  0.14,  -0.48)  and  that  of  camera  2 
is  (-0.011,  -0.163,  -0.48). 


Therefore,  T  is 
CA1 


0.0995 

0.995 

0 

0 


-0.9355 

0.341 

-0.011 

0.0842 

-0.0341 

0.14 

(4.72) 

0.343 

0.9392 

-0.48 

0 

0 

1 

The  unit  of  all  the  numbers  in  T..,is  the  meter. 

CA1 

Camera  2  was  mounted  to  the  frame  by  using  the  left  fixture, 
whose  structure  is  shown  in  Fig.  4.21.  By  going  through  the  same 

g 

derivation,  T  can  be  identified  as: 


0.0995 

0.995 

0 

0 


-0.9355 

0.341 

-0.011 

-0.0842 

0.0341 

-0.163 

0.343 

0.9392 

-0.48 

0 

0 

1 

(4.73) 


B  B 

It  should  be  emphasized  again  that,  for  both  T  and  T^,  the 
3x3  orientation  matrix  is  permanently  fixed  and  will  never  change 
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unless  new  fixtures  with  different  structures  are  used.  However,  the 
position  vector  may  vary  as  fixtures  are  moved  around.  Hence,  it  is 
necessary  to  measure  px,  py  and  pz  whenever  fixtures  are  relocated 
to  new  locations. 

Fig.  4.22  shows  a  picture  of  the  OSU  Hexapod  with  the  vision 
system  installed  on  it. 

4.8  Software  Organization  Of  Triangul ation 

The  conversion  from  image  coordinates  to  three-dimensional  spatial 
coordinates  is  summarized  in  a  flowchart  shown  in  Fig.  4.23.  Image 
coordinates  ( 1X1, I Yl, IX2.IY2)  are  provided  by  the  interface  circuit  and 
are  brought  to  the  computer  by  subroutine  V1D  as  described  in  Section 
3.6.  The  distorted  image  radii  IR1  and  IR2  are  computed  using  these 
data.  Distortion  compensation  is  then  performed  to  find  the  non- 
distorted  radius  RI  and  R2  by  calling  subroutine  R00T4.  After  the  un¬ 
distorted  images  are  located  on  both  frames,  projection  rays  VI  and  V2 
are  constructed  by  using  camera  models.  All  quantities  represented  in  1 
the  Camera  2  CS  are  transformed  to  the  Camera  1  CS  using  HTM  *T  . 
Specifically,  V2  is  represented  as  V2'  in  Camera  1  CS.  The  intersection 
of  VI  and  V2‘  gives  the  coordinates  of  the  object  point  in  Camera  1  CS. 

g 

They  are  then  transformed  to  the  body  CS  through  T  ^  . 

4.9  Summary 

Four  subjects  were  discussed  in  this  chapter,  including  camera 
calibration,  lens  compensation,  triangul ation,  and  identification  of  the 
geometrical  relationship  between  cameras  and  the  Hexapod  body- 
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Figure  4.23  Flowchart  of  Program  TRIANG 


The  camera  model  used  in  this  work  is  a  modified  pin-hole  model  in 
which  the  focal  plane  is  placed  before  the  lens  center.  Camera  cali¬ 
bration  was  performed  to  find  the  location  of  the  lens  center  with 
respect  to  the  focal  plane.  Ideally,  the  image  coordinates  of  a  lens 
center  should  be  (124,122).  However,  it  was  found  that  the  actually 
values  are  (119,118)  and  (111,113)  for  Camera  1  and  Camera  2,  respec¬ 
tively. 

The  wide-angle  lenses  introduced  considerable  image  distortion, 
which  was  represented  as  a  fourth  order  polynomial  function  of  the  un¬ 
distorted  image  radius  R.  Lens  compensation  was  performed  on  the  di¬ 
storted  image  coordinates  so  that  the  corrected  image  coordinates  are 
approximately  1  pixel  away  from  the  undistorted  values. 

The  three-dimensional  coordinates  of  an  object  point  were  recon¬ 
structed  from  the  compensated  image  coordinates  of  both  cameras  using 
triangul ation  method.  Coordinate  transformations  were  performed  to 
bring  all  mathematical  entities  from  Camera  2  CS  to  Camera  1  CS  and  from 
Camera  1  CS  to  body  CS.  The  effect  of  tilt  and  roll  angles  on  the 
common  field  of  view  was  also  presented. 

The  geometrical  relationship  between  cameras  and  the  vehicle  body 
was  identified  by  analyzing  the  physical  structure  of  the  fixtures 
through  which  cameras  were  mounted  onto  the  Hexapod  body. 
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Chapter  5 


ALGORITHMS  FOR  EVEN  TERRAIN  LOCOMOTION 

5.1  Introduction 

As  described  in  Chapters  3  and  4,  the  binocular  vision  system 
enables  the  Hexapod  to  perceive  a  specified  foothold  and  the  triangu¬ 
lation  technique  helps  the  vehicle  to  locate  the  foothold  with  respect 
to  its  current  position.  The  next  task  the  machine  should  do  is  to  move 
in  such  a  way  that  its  right/left  front  foot  steps  on  the  designated 
footnold.  This  difficult  task,  which  includes  the  control  of  each 
individual  joint  and  the  coordination  of  all  18  joints  in  real-time,  is 
handled  by  sophisticated  software  algorithms  inside  a  PDP-11/70  com¬ 
puter.  Functionally  speaking,  the  software  may  be  divided  into  three 
levels.  The  lowest  level,  which  deals  with  the  servo  control  loop  in 
generating  commanded  joint  rate  voltages  by  using  a  linear,  error-driven 
scheme,  is  described  in  detail  in  [56].  The  intermediate  level,  called 
the  motion  planning/execution  level,  is  devoted  to  the  computation  of 
the  desired  position  and  velocity  of  each  leg  at  each  instant  as  seen 
from  the  center  of  the  body.  This  involves  the  formulation  of  the 
kinematic  equations  which  convert  the  Cartesian  coordinates  of  a  foot 
into  the  three  corresponding  joint  angles  and  the  reverse,  the  utili¬ 
zation  of  the  inverse  Jacobian  matrix  to  obtain  the  desired  joint  rates 
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from  linear  rates,  the  specification  of  the  behavior  of  a  supporting  leg 
as  viewed  from  vehicle  body  as  the  Hexapod  is  moving,  and  the  generation 
of  the  trajectory  of  a  foot  in  the  transfer  phase.  A  complete  and  de¬ 
tailed  discussion  of  the  above  issues  is  given  in  [56].  The  activities 
at  the  top  level,  namely  the  locomoti on-cycle  level,  will  be  considered 
in  this  chapter.  In  Section  5.2,  a  particular  gait  suitable  for  FTL 
control  is  selected.  The  questions  as  to  when,  where,  and  how  the  body 
should  move  are  addressed  in  Section  5.3.  Particul arly ,  a  finite-step 
heuristic  algorithm  is  devised  to  guide  the  center  of  body  so  that  the 
stability  margin  is  optimized.  Section  5.4  explains  the  checking  of  the 
kinematic  limit  of  every  joint  to  prevent  the  Hexapod  from  getting  into 
an  undesired  situation.  Two  approaches  to  updating  the  coordinates  of 
the  footholds  as  the  Hexapod  is  moving  are  discussed  in  Section  5.5. 

5.2  Selection  of  a  Proper  Gait 

A  gait  is  a  sequence  of  events  of  leg  lifting  and  leg  placing.  The 
problem  of  choosing  a  gait  is  to  determine,  within  a  locomotion  cycle, 
which  leg  should  be  lifted  up,  how  long  it  stays  in  the  transfer  phase 
before  being  placed  down,  and  which  leg  should  be  lifted  up  next.  With 
the  gait  defined,  the  legs  in  the  supporting  phase  and  in  the  transfer 
phase  are  known  so  that  appropriate  treatment  is  applied  to  each  leg. 

For  a  six-legged  vehicle,  there  are  millions  of  possible  gaits  [4]. 
However,  only  a  small  number  of  them  are  suitable  for  FTL  operation.  In 
this  mode,  the  middle  legs  have  to  step  to  the  locations  which  were 
occupied  by  the  front  legs,  and  the  rear  legs  must  follow  the  footprints 


of  the  middle  legs.  Thus,  in  the  same  locomotion  cycle,  the  front  legs 
ought  to  move  before  the  middle  legs,  and  the  rear  legs  should  act  only 
after  the  middle  legs  have  completed  the  transfer  phase;  otherwise  leg 
interference  will  occur.  With  the  assumption  that  only  one  leg  moves  at 
a  time,  all  gaits  that  satisfy  the  above  requirement  are  shown  in  Fig. 
5.1.  Note  that  the  legs  of  the  Hexapod  is  labeled  in  the  following  way: 
leg  1  is  the  left,  front  leg;  leg  2  is  the  right,  front  leg;  leg  3  is 
the  left,  middle  leg;  leg  4  is  the  right,  middle  leg;  leg  5  is  the  left, 
rear  leg;  and  leg  6  is  the  right,  rear  leg.  An  inspection  of  the  tree 
in  this  figure  reveals  that  the  right  and  left  subtrees  are  mirror 
images  of  each  other;  i.e.,  by  replacing  ’2'  with  *  1 ‘ ,  ‘4 ‘  with  ’3’, 
and  ‘ 6 1  with  ‘S',  the  right  subtree  becomes  identical  to  the  left  one. 
Due  to  this  symmetry,  it  is  sufficient  to  consider  only  one  of  the  sub¬ 
trees  for  further  analysis.  Among  the  10  sequences  in  the  left  subtree, 
two  are  especially  interesting.  One  is  the  sequence  1-3-5-2-4-5  and  the 
other  is  1-2-3-4-5-6.  The  former  represents  the  case  in  which  all  three 
legs  on  the  left  side  of  the  vehicle  move  before  the  motion  switches  to 
the  right  hand  side.  For  the  latter  sequence,  legs  on  each  side  move  in 
an  alternating  way.  The  rest  of  the  sequences  are  combinations  of  these 
two  basic  sequences.  Furthermore,  for  purpose  of  this  dissertation, 
sequence  1-3-5-2-4-6  is  preferred  over  1-2-3-4-5-6  because  it  has 
"physical  symmetry",  which  is  an  important  property  and  is  commonly 
observed  in  animal  motions  [4].  A  thorough  study  of  this  class  of  gaits 
(regular  symmetric  gaits)  has  been  performed  by  Sun  [39].  Therefore, 
1-3-5-2-4-6  was  chosen  as  the  gait  to  realize  FTL  motion. 
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5.3  Navigation  of  the  Body 


5.3.1  Timing--When  Should  the  Body  Move 

With  the  sequence  1-3-5-2-4-6,  a  locomotion  cycle  begins  at  the 
moment  when  leg  1  is  lifted  up  and  ends  when  leg  6  is  placed  down.  The 
timing  which  governs  the  motion  of  each  leg  is  well  defined.  The  ques¬ 
tion  that  remains  to  be  answered  concerns  the  motion  of  the  body  with 
respect  to  time;  that  is,  whether  it  should  move  at  a  constant  velocity 
throughout  the  whole  cycle  as  in  the  case  of  wave  gait  locomotions  [4], 
or  whether  the  speed  should  be  nonuni  form  and  the  body  would  only  move 
for  a  portion  of  a  cycle.  Investigation  of  this  issue  requires  the  fol¬ 
lowing  definitions.  Definitions  1  and  2  were  given  by  McGhee  in  [4]. 

Definition  1:  the  supporting  polygon  of  a  mobile  vehicle  is  a  two- 
dimensional  point  set  in  a  horizontal  plane  consisting  of  the  convex 
hull  of  the  vertical  projection  of  all  points  of  contact  of  each  sup¬ 
porting  foot. 

Definition  2:  the  longitudinal  stability  margin  of  a  supporting 
polygon  is  the  shortest  distance  from  the  vertical  projection  of  the 
center  of  gravity  to  an  edge  of  this  polygon  as  measured  in  the  line  of 
travel . 

The  timing  problem  mentioned  above  was  evaluated  in  terms  of  the 
longitudinal  stability  margin,  and  the  following  example  should  provide 
some  insight  to  this  issue. 

For  simplicity,  only  straight-line  motion  will  be  considered  and  it 
will  be  assumed  that  only  one  leg  is  in  the  transfer  phase  at  a  time. 
Fig.  5. 2. a  shows  the  position  of  the  center  of  body  and  the  supporting 
polygon  when  leg  1  is  moving  forward.  The  front  boundary  of  the  polygon 
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is  defined  by  the  foot  tips  of  leg  2  and  leg  3,  which  remain  fixed 
before  leg  1  is  placed  down.  Clearly,  if  the  body  moves  simultaneously 
with  leg  1,  its  center  would  become  closer  and  closer  to  line  32,  which 
means  that  the  longitudinal  stability  margin  is  strictly  decreasing. 
Apparently,  this  is  not  a  good  choice.  For  the  same  reason,  a  similar 
outcome  would  be  observed  if  the  body  tried  to  move  while  leg  2  is 
moving.  Hence,  it  is  concluded  that  the  body  should  not  move  when  leg  1 
or  leg  2  is  in  the  transfer  phase. 

Fig.  5.2.b  represents  the  case  in  which  leg  3  is  in  the  air.  It  is 
evident  that,  compared  with  the  front  legs  or  the  rear  legs,  the  middle 
legs  have  very  little  influence  on  the  shape  of  the  polygon  as  well  as 
the  stability  margin,  especially  the  longitudinal  margin. 

The  next  figure,  Fig.  5.2.c  shows  the  case  in  which  the  body  has 
not  moved  since  the  beginning  of  the  locomotion  cycle  and  leg  5  is  in 
motion.  The  center  of  the  body  is  very  close  to  the  rear  border  36  and 
the  machine  is  in  a  dangerous  position.  This  suggests  that  the  vehicle 
oody  should  move  forward  some  distance  before  leg  5  takes  off.  Indeed, 
by  doing  so,  the  stability  margin  is  greatly  improved  as  seen  in  Fig. 
5.2.d. 

The  timing  diagram  shown  in  Fig.  5.3  summarizes  the  foregoing 
discussion.  It  shows  the  intervals  for  leg  movement,  body  motion  and 
visual  data  acquisition.  Events  take  place  in  a  purely  sequential 
fashion.  Since  all  legs  are  in  the  supporting  phase  when  the  body  is 
moving,  this  gait  has  the  advantage  of  longitudinal  stability  margin  in 
addition  to  programming  simplicity.  The  drawback  is  that  it  is  slow. 

The  length  of  a  cycle  can  be  reduced  by  allowing  two  events  to  occur 
concurrently .  Fig.  5.4  shows  that  body  motion  is  overlapped  with  the 
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Figure  5.5  Timing  Diagram  with  Overlapping  of  Body  Motion,  Leq  Motion 
and  Foothold  Fetch. 


action  of  the  middle  legs.  Foothold  acquisition  may  also  be  overlapped 
with  other  events  as  shown  in  Fig.  5.5.  This  requires  the  multitasking 
of  motion  planning/execution  module  and  vision  software. 

In  terms  of  the  software  implementation,  these  three  events  were 
defined  as  LEGMOTION,  B00YM0TI0N  and  FOOTHOLDFETCH.  The  flow  of  the 
events  and  the  transitions  from  one  to  another  are  shown  in  the  flow¬ 
chart  in  Fig.  5.6.  The  activity  represented  by  the  flowchart  is  added 
to  the  existing  motion  planning/execution  loop.  Upon  entering,  the 
present  event  is  identified  and  then  the  corresponding  action  follows. 
For  a  FOOTHOLDFETCH  event,  the  vision  subroutine  is  invoked  and  tri¬ 
angulation  is  performed  to  compute  the  spatial  coordinates  of  the  next 
foothold.  The  V  coordinate  is  used  to  distinguish  footholds  for  leg  1 
and  leg  2.  A  variable,  LEGLIFT,  is  defined  to  indicate  the  particular 
leg  that  is  currently  in  the  transfer  phase.  At  the  completion  of 
FOOTHOLDFETCH,  activity  is  switched  to  LEGMOTION.  Each  leg  spends  a 
fixed  amount  of  time  (determined  by  parameter  LIFTTIME)  in  the  transfer 
phase.  If  time  has  not  expired  yet,  it  continues  to  move  toward  the 
specified  foothold.  When  the  time  is  up,  the  next  event  has  to  be 
determined  depending  on  the  transferring  leg.  If  it  is  either  leg  1  or 
leg  2,  the  following  event  is  still  LEGMOTION,  and  LEGLIFT  is  updated  as 
leg  3  or  leg  4,  respectively .  If  it  is  leg  3  or  leg  4,  30DYM0TI0N  will 
be  the  next  event.  Otherwise,  it  must  be  either  leg  5  or  leg  6  and 
FOOTHOLDFETCH  should  follow.  The  vehicle  body  is  allowed  to  move  for  a 
period  of  time  specified  by  the  parameter  PERIOD.  When  this  is  done, 
the  next  event  becomes  LEGMOTION  again  and  LEGLIFT  is  modified.  The 
destination  toward  which  the  bo<fy  is  moving  is  discussed  in  the  next 
section. 
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Figure  5.6  Flowchart  of  a  Sequential  FTL  Motion. 
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5.3.2  L)estination--Where  Should  the  Body  Move 


For  FTL  operation,  destination  of  each  leg  is  its  foothold  (FTHD) , 
which  is  specified  explictly  as  follows: 

FTH0[ I ,  K  +  1]  =  FTHD[I-2,  K]  for  I  =  3,  4,  5,  6 

FTHD[I ,  K  +  1]  =  Assigned  by  operator  for  1=1,2  (5.1) 

where  I  is  the  leg  number  and  K+l  indicates  the  (K+l)th  locomotion 
cycle.  The  next  supporting  polygon  is  actually  known  before  the  next 
cycle  starts  once  the  footholds  are  selected  by  the  operator.  This  is 
illustrated  in  Fig.  5.7,  where  the  present  polygon  is  defined  by  points 
1,2, 3, 4, 5  and  o  and  the  next  polygon  is  defined  by  points  a, b, 1,2, 3  and 
4. 

While  tne  destinations  of  legs  are  well  defined  from  one  cycle  to 
another,  the  destination  for  the  center  of  the  body  is  less  clear.  If 
tne  current  location  of  the  center  is  at  point  C  (Fig.  5.7),  it  is 
necessary  to  find  a  point  C‘  within  the  next  polygon  as  its  destination. 
The  following  study  attanpts  to  find  a  point  which  would  produce  the 
maximal  stabil i ty  margin. 

Before  proceeding  any  further,  an  important  distinction  must  be 
made  here  to  distinguish  the  optimization  procedure  to  be  presented  and 
the  theory  which  has  been  proved  for  optimally  stable  wave  gaits.  In 
1973,  Umnov  and  Bessonov  [57]  proved  that  for  forward,  straight-line 
locomotion  with  a  given  duty  factor  (the  duty  factor,  e  i ,  of  leg  i  is 
the  fraction  of  a  locomotion  cycle  during  which  leg  i  is  in  contact  with 
the  supporting  surface),  there  is  a  wave  gait  which  gives  an  optimal 
longitudinal  stability  margin.  An  optimally  stable  wave  gait  has  the 
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Figure  5.7  Present  and  the  Next  Supporting 
Polygons . 


Figure  5;8  Relationships  Between  Periodic  Gaits 
Wave  Gaits,  and  FTL  Gaits. 


following  properties:  (l)  it  is  symmetric,  and  (2)  the  phase  difference 
from  front  to  back  leg  on  each  side  is  equal  to  1-p  (p  is  the  duty 
factor  for  all  legs),  which  implies  that  the  rear  legs  move  before  the 
middle  legs  and  the  middle  legs  act  before  the  front  legs  on  both  sides. 
According  to  this  definition,  the  FTL  gait  (1-3-5-2-4-6)  is  not  an  opti¬ 
mally  stable  wave  gait.  Consequently,  for  any  p,  the  stability  of  a  FTL 
gait  is  less  than  that  of  a  wave  gait.  In  fact,  FTL  gaits  and  wave 
gaits  are  two  di fferent  families  of  gaits  whose  intersection  is  empty, 
as  illustrated  in  Fig.  5.8.  While  Umnov  and  Bessonov  showed  that  the 
wave  gait  is  optimal,  among  all  possible  gaits,  for  a  special  type  of 
locomotion  (forward,  straight-line  motion) ,  the  optimal  algorithm 
developed  in  the  next  section  attempts  to  find  an  optimal  stability 
margin  for  any  given  gait  with  respect  to  any  kind  of  motion.  Thus,  the 
optimal  algorithm  can  be  applied  to  FTL  gaits  as  well  as  any  other  kind 
of  gaits. 

5-3.2. L  The  Optimally  Stable  Point  (OSP) 

A  few  definitions  are  needed  in  order  to  identify  the  problem 
associated  with  finding  of  an  optimal  destination  for  the  body. 

Definition  3:  The  stability  margin  of  a  legged  vehicle  is  the 
shortest  distance  from  the  vertical  projection  of  the  center  of  gravity 
of  the  body  to  each  side  of  the  supporting  polygon. 

Note  that  the  stability  margin  is  an  extention  of  the  longitudinal 
staoility  margin  by  considering  not  only  the  margin  in  the  line  of 
travel,  but  also  margins  in  all  other  directions. 
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Definition  4:  The  Optimally  Stable  Point  (OSP)  of  a  supporting 
polygon  is  the  interior  point  that  produces  the  maximal  stability  margin 
when  the  center  of  gravity  is  projected  onto  it. 

With  these  definitions,  the  problem  to  be  solved  may  be  stated  as 
follows:  given  an  arbitrary,  convex,  N-sided  polygon,  find  its  OSP. 

Note  that  for  Hexapod  locomotions,  N  may  be  3,  4,  5  or  6,  while  for 
a  general  U- 1  egged  walking  machine,  N  may  be  greater  than  6. 

Three  approaches  were  considered  in  computing  the  OSP  of  an  arbi¬ 
trary  polygon.  They  are  described  below. 

5. 3. 2. 2  The  Analytical  Method 

This  approach  first  determines  the  equation  of  each  side  and  then 
expresses  the  distance  from  an  arbitrary  point  (x,y)  inside  the  polygon 
to  each  side  as  a  function  of  x  and  y.  Generally,  N  distances,  denoted 
as  [di,d2, . . .  ,dn]  would  be  obtained.  The  minimum  of  them  is  then 
maximized.  A  similar  problem,  which  requires  the  mi nimizati on  of  the 
maximal  joint  angle  of  a  multi-jointed  manipulator,  has  been  solved  by 
Klein  and  Huang  based  on  the  i-norm  concept  [58].  Although  an  exact 
solution  requires  that  a  be  infinity,  they  demonstrated  that  the  result 
obtained  with  i  being  6  is  close  enough.  While  their  problem  was  the 
ini nimi zation  of  a  maximum  quantity,  the  problem  encountered  here  is  to 
maximize  a  minimum  value.  However,  the  latter  can  be  transformed  into 
the  former  by  substracting  each  variable  from  a  constant.  That  is,  to 
maximize  the  minimum  of  [di.dg, .. .  ,dn],  these  N  quantities  can  be  first 
transformed  into  [c-di ,c-d2, • • .  ,c-dn].  If  di  is  the  minimum  of 

[d]_,d2 . dn] ,  then  c-d-j  will  be  the  maximal  of  [c-di,c-d2, . • . ,c-dn]. 

Thus,  the  minimization  of  c-d-j  implies  the  maximization  of  d-j .  To 
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/  minimize  c-di ,  the  i -norm  of  [c-di,c-d2, . • . ,c-dn]  is  used  as  a  cost 

function,  whose  partial  derivatives  with  respect  to  x  and  y  are  set  to 
zero,  respectively ,  to  generate  the  equations  needed  for  a  minimal  con¬ 
dition.  If  these  equations  can  be  solved  analytically,  the  one  can  take 
the  limit  as  i  goes  to  infinity  to  find  the  result.  Otherwise,  dif¬ 
ferent  values  of  i  may  be  substituted  into  the  equations  and  numerical 
methods  are  then  employed  to  compute  the  solutions.  An  example  which 
illustrated  the  foregoing  method  is  provide  in  [59]. 

As  mentioned  above,  i  must  be  infinity  to  produce  an  exact  solu¬ 
tion,  and  there  is  no  guarantee  for  a  closed-form  solution.  However,  if 
a  siaal  ler  value  of  t ,  say  6,  can  always  yield  a  result  that  is  close 
enough,  this  approach  may  be  considered  to  compute  the  OSP  for  real-time 
control  envi ronments. 

5. 3. 2. 3  The  Geometrical  Method 

This  approach  is  best  illustrated  by  examples.  Consider  the  case 
of  a  triangle,  which  is  the  simplest  polygon.  It  is  evident  that  the 
OSP  exists  and  is  unique  for  all  triangles.  The  OSP  is  located  at  the 
intersection  of  the  three  angular  bisectors  as  shown  in  Fig.  5.9. 
Therefore  the  problem  of  finding  the  OSP  is  solved  for  any  tripod-gait 
locomotion.  Also  note  that  the  distances  from  this  point  to  all  sides 
are  the  same.  This  observation  leads  to  the  following  theorem: 

Theorem  1:  If  there  exists  one  point  0  inside  a  polygon  P  such 
that  the  distance  from  0  to  each  side  is  the  same  and  is  equal  to  S, 
tnen  Q  is  the  optimally  stable  point  and  S  is  the  maximal  stability 
margin  of  P.  Furthermore,  0  is  located  at  the  intersection  of  all  the 
angular  bisectors  of  P. 
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Figure  5.9  The  Intersection  of  the  Three  Bisectors 
Gives  the  Optimally  Stable  Point. 


8  C  D 


0 


Figure  5.10  Portion  of  a  Polygon  Whose  Angular 

Bisectors  Intersect  at  a  Single  Point  0. 


Proof:  refer  to  Fig.  5.10,  which  shows  a  portion  of  polygon  P. 


It  is  obvious  that  aABO  is  similar  to  aCBO,  thus  0  lies 
along  the  angular  bisector  of  angle  ABC.  Indeed,  by 
connecting  0  to  any  vertex,  it  is  apparent  that  0  is  a 
point  along  that  bisector.  Therefore,  0  is  located  at  the 
intersection  of  all  bisectors. 

Since  0  is  the  intersection  of  all  bisectors,  by 
connecting  0  to  all  vertices  gives  M  triangles,  and  0  is 
the  common  vertex  of  all  triangles.  If  point  0  is  moved 
away  from  its  location,  it  would  be  inside  a  triangle,  say 
A  BOD.  It  is  evident  that  the  distance  from  0  to  side  BD 
is  now  less  than  S.  Hence,  0  must  be  the  OSP.  Q.E.D. 

Definition  5:  A  polygon  whose  angular  oi sectors  intersect  at 
unique  point  is  called  a  "BI  polygon".  Tne  set  of  all  BI  polygons 
i s  denoted  as  8IP. 

Definition  6:  A  polygon  is  regul  ar  if  and  only  if  (1)  all  its 
sides  have  the  same  length,  and  (2)  all  its  internal  angles  are  equal. 

From  Theorem  1,  it  is  evident  that  the  geometrical  method  may  be 
applied  to  only  the  set  of  BIP,  which  is  only  a  small  portion  of  the  set 
of  all  polygons.  Note  that  the  set  of  regular  polygons  is  a  subset  of 
BIP.  One  interesting  property  of  a  regular  polygon  is  that  the  OSP  is 
also  its  geometric  center,  which  is  defined  as  follows: 

Definition  7:  The  coordinates  (XgC,YgC)  of  the  Geometric  Center 
(GC)  of  a  N-sided  polygon  is  defined  as  follows: 
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Xgc  =  (XX+X2+...+X,,  )/N 

Ygc  =  ( Y i +Y 2  +  -  *  - +Y n  )/M 


(5.2) 


where  X,  and  Yj  are  the  coordinates  of  the  ith  vertex. 

Theorem  2:  The  angular  bisectors  of  a  regular  polygon  P  inter¬ 
sect  at  a  single  point  0,  which  is  also  the  geometric  center  of  P. 

Proof;  assume  that  ?  has  N  sides,  its  N  vertices  are  labeled  as 

Vq  through  Vn_i,  and  the  coordinate  system  is  defined  in  such 
a  way  that  0  is  the  origin  and  the  X  axis  passes  Vq.  It  is 
also  assumed  that  the  distance  from  0  to  each  vertex  has 
been  normalized  to  oe  1.  This  arrangement  is  shown  in  Fig. 
5.11.  It  is  evident  that  the  Cartesian  coordinates  of  are 
(cos(ke),  sin(ke))  where  e  is  2u/N,  thus 

xgc  =  ^  lcos(  J )  +  cos(9  )  +  cos(2e)  +  ...  +  cos(n-l)e]  (5.3) 

Ygc  =  ^  [sin(U)  +  sin(e  )  +  si n( 2e  )  +  ...+  sin(n-l)o]  (5.4) 

Eqs.  (5.3)  and  (5.4)  resemble  the  following  trigonometric 
equations 

cos<|)  +  cos(«t>  +  a  )  +  cosU  +  2a)  + - +  cos  (<t»  +  (n-l)a  ) 

.  n« 
sin  — - 

-  - — -  .  COS  (^  +  -~  a)  (5.5) 

.  _  a  2 
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sine  +  si n(<p  +  a  )  +  sin(<j>  +  2a  )  + - +  sin  ( <j>  +  (n-l)a) 


fia 

Sin  2  n-1 

- •  sin  (<j>  +  —  a  ) 

a  L 

sm  - 


Comparing  them  reveals  that 


'gc 


sin  n  , n-1  , 

-  .  cost - it)  =  J 

rt  n 

si  n  — 
v 


(5.6) 


(5.7) 


gc 


sin  n:  .  ,  n-1  , 

-  .  sin( - it)  =  u 

n  n 


si  n 


N 


(5.8) 


Therefore  0  is  also  the  GC  of  P.  Q.E.D. 

To  show  that  this  method  can  not  be  generalized  to  handle  an 
arbitrary  polygon,  a  counter-example  is  provided  in  Fig.  5.12,  where 
the  polygon  is  a  rectangle.  It  is  evident  that  in  this  case  the  four 
angular  bisectors  do  not  intersect  at  one  point.  Moreover,  the  OSP  is 
not  unique  since  every  point  along  the  segment  AB  will  yield  the  same 
stability  margin  which  is  with  respective  to  L2  and  L4.  This  example 
brings  out  two  points:  (a)  the  bisectors  of  an  arbitrary  polygon 
generally  do  not  intersect  at  one  point,  and  (b)  the  OSP  may  not  be 
unique. 

Since  the  geometrical  approach  of  finding  the  intersection  point 
can  only  be  applied  to  8IP,  a  more  general  method  is  needed  to  deal  with 
an  arbitrary  polygon.  This  motivation  leads  to  the  development  of  the 
following  algorithm. 


168 


5. 3.  2.  4  A  Heuristic  Algorithm 

More  definitions  are  needed  in  order  to  describe  this  algorithm. 
Defini  tion  8:  Two  polygons  Pj_  and  P2  are  similar  to  each  other  if 
and  only  if  (1)  every  pair  of  corresponding  angles  are  equal,  and  (2) 
tne  ratio  of  the  length  of  the  correspondi ng  sides  is  a  constant,  i.e.. 


In  1 

!v2'i 

1  1 

!v3‘I 

1  1 
1 

k'l 

n  1 

r  | 

v2 

|v3  i 

r  =  ••••  =  I 

i  1 

where  Vi  and  Vi'  are  side  i  of  PI  and  P2,  respectively.  The  similarity 
between  PI  ana  P2  is  denoted  as  P2=r(Pl). 

Definition  9:  To  "shrink"  a  polygon  P  by  D,  a  line  is  drawn  in 
parallel  to  each  individual  side  and  is  moved  D  inward  from  the  original 
side,  as  shown  in  rig.  5.13.  The  shrunken  polygon  is  denoted  as  SP. 

Definition  10:  SP  is  degenerated  if  P  is  an  N-sided  polygon  while 
SP  has  less  than  N  sides. 

To  fulfill  this  algorithm,  the  following  procedures  should  be 
followed  after  an  initial  polygon  (IP)  is  given: 

(1)  let  K  =  1  and  P ( 1 )  =  IP,  where  the  index  .< 
indicates  the  number  of  iterations. 

(2)  Compute  the  geometric  center  of  P(K). 

(3)  Compute  the  distances  from  GC  (Geometric  Center)  to  each 

side  of  P(K ) .  n  distances,  (dj ,d2, . . .  ,dn) ,  would  be 
obtained.  Let  the  minimal  of  them  be  D^,  i.e.,  = 

MIH(di,d2,...,dn). 
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(4)  "Shrink"  P(K)  by  and  denote  the  shrunken  polygon  as 
SP(K).  Note  that  SP (K )  may  be  (i)  "similar"  to  P(K), 

(ii)  degenerated,  or  (iii)  neither  (i)  nor  (ii). 

(5)  Examine  SP(!<)  to  see  if  it  can  be  further  shrunk. 

Since  the  most  simple  type  of  polygon  is  a  triangle, 

SP(K)  has  to  be  at  least  a  triangle  or  have  more  than 
three  sides  to  continue  the  shrinking  process.  If  SP(K) 
can  be  processed  further,  let  K  =  K+l,  P(K)  =  SP(K-I) 
and  repeat  steps  (2)  through  (4).  Otherwise,  SP(K)  may 
be  a  point  or  a  segment,  then  go  to  step  (5)  and  the 
process  is  terminated. 

(6)  The  optimal  stability  margin  is 

M 

Dosm  =  ?  3k  (5.10) 

k=l 

The  following  discussion  is  intended  to  show  that  the  above  algor¬ 
ithm  indeed  generates  an  optimal  stability  margin.  The  analysis  is 
divided  into  two  parts,  one  deals  with  the  cases  where  J‘U) ,  t*.*  Tirst 
shrunken  polygon,  is  similar  to  IP,  and  the  other  handles  situations  in 
which  SPU)  is  NOT  similar  to  IP. 

b.3.2.2  Cases  Where  SP ( 1 )  is  Similar  to  IP 

Tneorein  3:  SP(1)  is  similar  to  IP  if  and  only  if  the  term 

cot(  !*■  )  +  cot( 

(77] 
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is  a  constant  for  every  side  of  IP.  The  parameter  |Vi|  is  the  length  of 
side  i,  and  e and  +  1  are  the  two  angles  associated  with  it. 

Proof:  Let  and  V  De  the  ith  side  of  IP  and  SP ( 1 ) .  Refer  to 

Fig.  5.14,  which  shows  portion  of  IP  and  SP ( 1 )  with  Vj  and  V-j’ 
Doth  present.  If  IP  is  shrunk  by  D  to  produce  SP ( 1 ) ,  it  is 
clear  that 

I  Vi  i  =  IV  I  +  a  +  b 


IV  I  +  ^  ’  cot{^-)  +  D  •  cot(— ^ — ) 


«  I  Vi 1  |  +  D  •  [cott^)  +  cotl^p-)]  (5.11) 


Rearranging  Eq.  (5.11)  gives 


IV  -  0  •  [cot(|i)  +  cot(^2~”") ] 


(5.12) 


Hence  the  ratio  between  | V i *  I  and  | Vi  |  is 


jy . ,  I  | V i '  I  -  0  ♦  [cot(^i)  +  cot(-* — )] 


=  1  -  D  [cot(rp-)  +  cot(^~-)  ]/  I V i  | 


(5.13) 


Since  l)  is  the  same  for  each  pair  of  sides,  the  ratio  for  any  pair  of 
sides  would  be  the  same  if  the  term 


cot(  )  +  cot(^~) 


f 


is  a  constant.  If  this  the  case,  then  SP ( 1 )  is  similar  to  IP  by 
defi ni tion  b.  Q.E.D. 


Theoran  4:  If  P2  =  r(Pj_),  Cdj. ,d2,  •  *  •  ,dn]  is  the  distances  from  the 
GC  of  Pi  to  its  sides,  and  [d^ ' ,d2 ' , • . .  ,dn' ]  is  those  of 
P2,  then 

=  ...  =  =  r  (5.14) 

di  di  dn 

Proof:  One  may  assign  a  coordinate  system  to  Pi 

and  P2»  respecti vely .  The  origin  of  this  coordinate 
system  is  at  vertex  1,  and  each  side  is  represented  as  a 
vector,  as  shown  in  Fig.  5.15.  Following  this  notation, 
the  Cartesian  coordinates  of  the  vertices  of  Pi  are: 

1 ( x  ,y )  =  (0,0), 

2(x,y )  =  Vi, 

3(x,y)  =  Vi  +  V2, 

n(x,y)  =  Vi  +  V2  +  •••  +  Vn_i.  (5.15) 

and  those  of  P2  are: 

1‘ (x,y)  =  (0,0), 

2'(x,y)  »  VT', 

3‘  (x,y)  •  V 

n'(x,y)  =  V  +  v7'  +  ...  +  Vl'-  (5*16) 
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Thus  the  coordinates  of  the  GC  of  PA  are 

GC(x,y)  =  |G  +  +  (V^  +  V2)  +  ...  +  ( V  ^  +  V2  +  ...  +  Vn_^)]/M 

=  C(n-l)v7  +  ( n-2 )  V2"  +  ...  +  V~^]/N 
=  ( XgC >  Ygc)  (5.17) 

and  those  of  Pj  are 

GC'(x.y)  =  [(n-l)v7‘  +  (n-2)v7'  +  ...+V^']/N 

=  ^ X g c 1  »YgC'  )  (5. 18) 

Si  nee  ?2  =  r( P 1 ) ,  thus 
!  V-j  ‘  I 

=  Y 

I  Vi  I  (5.19) 

Substituting  Eq.  (5.19)  into  (5.18)  and  comparing  the  result  with  (5.17) 
reveals  that 

Xgc'  =  r  '  Xgc 

Ygc'  =  r  •  Ygc  (5.20) 

Let  the  equation  of  in  coordinate  system  be 

a  X  +  b  Y  =  c  (5.21) 

Since  this  line  passes  through  the  origin,  thus  c  must  be  zero  and  the 
equation  becomes 

a  X  +  b  Y  =  0  (5.22) 
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Since  is  parallel  to  V^,  the  equation  of  V^‘  in  P£  coordinate  system 
i s  also  a  X  +  b  Y  =  J.  Tne  distance  from  a  point  (x,y)  to  a  line  aX  + 
bY  =  c  is 


lax  +  by  -  cl 

^a2  +  &2  (5.23) 

Hence 

_  Id  Xgc  +  b  Ygc 1 

/a2  +  b2  (5.24) 

d •  =  la  Xgc1  +  b  Ygc1 I  _  r  la  Xgc  +  b  Ygc] 

/a2  +  b2  /a2  +  b2  (5.25) 

Therefore 

dl'  -  r  d!  (5.26) 

By  applying  the  same  technique  to  each  vertex  and  each  side 
Theorem  4  is  proved.  Q.E.O. 

Corollary  1:  if  P2  =  r(P1),  D=MI.NI[d1  ,de . dn],  and 

D' =MIN[di ' ,d£ ' , • .  •  ,dn' ],  then 

Dl  _  |V,  '  |  _ 

D  IV,  |  "  P  (5.27) 

for  i  =  1  to  N. 
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Tneoran  5:  if  SP(l)=r(lP),  then  SP(K)=r(P(l<) )  and  D|<+i  =  rO^. 

Proof :  This  statement  can  be  proved  by  induction  as  follows: 

(i)  the  case  of  N  =  1  holds  by  the  given  assumption  SP(l)=r(IP) 
and  by  Corollary  1. 

(ii)  Suppose  that  it  is  true  for  N  =  K,  i.e.,  SP(K)=P(K+l)=r(P(K) ), 
what  remains  to  be  verified  is  that  it  is  also  true  for 

N  =  K+l,  i.e,  SP(K+1 )=P (K+2)=r(P(X+l ) ).  Let  Vj ,  V, ' ,  and  V, " 
be  the  corresponding  side  of  P{K),  P(X+1),  and  P(I(+2),  and 
that  P(K)  is  shrunk  by  to  produce  P(l<+1)  while  P(K+1)  is 
shrunk  by  0^+  1  to  produce  P(K+2).  From  Fig.  5.14  and  Eq.  (5.13), 
the  following  equations  hold: 


Dk  cot(  )  +  cot(^~“ 
*  2  2 


D|<+i  cot(  1^-  )  +  cot(^~) 


(5.28) 


(5.29) 


I  Vi" I  _  |  Vi '  I 

I  Vi  *  I  I  Vi  1 


(5.30) 


(5.31) 
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Hence,  by  the  definition  of  similarity,  P(K+2  )  =  r(  P  (K+l ) ) ,  and 
ak+l  =  y  *  from  Corollary  1. 

(iii)  the  statement  holds  for  any  value  of  K  by  induction.  Q. E.D. 

Theoran  6:  if  SP(l)  =  r(IP),  then  IP  will  be  shrunk  into  a  single  point 
after  the  shrinking  process  has  been  applied  an  infinite  number  of 
times,  and  the  optimal  stability  margin  is 

ao 

Dosm  =  J  Dk  =  D1  +  rDl  +  r2  D1  +  r3  Dl  +  ••• 
k=l 

=  Oi/d-r)  (5.32) 

Proof:  According  to  Theorem  5,  if  SP(l)=r(IP),  then  every  time  the 

shrinking  operation  is  applied  to  a  polygon  P(K),  SP(K)  is  still 
similar  to  P(K)  out  is  reduced  by  a  factor  of  r.  Mathematical  - 
ly,  it  ta<es  an  infinite  number  of  times  of  shrinking  to  reduce 
IP  into  a  point  0.  Thus  Eq.  ( 5 . 1U)  becomes  Eq.  (5.32).  Q.E.D. 
Theorem  7:  if  SP ( 1 )  =  r( IP),  then  IP  is  a  member  of  31 P  and  the  OSP  is  the 
intersection  point  of  all  bisectors. 

Proof:  by  Theoran  b,  if  SP ( I )  =  r(IP),  then  IP  will  eventually  be 

shrunk  into  a  single  point  0.  Since  every  side  of  IP  approaches 
to  0  by  the  same  amount  of  distance  each  time  the  shrinking 
operation  is  applied,  the  distances  from  0  to  all  sides  are 
equal  and  have  a  value  of  Dosm.  Thus  the  angular  bisectors  of 
IP  intersect  at  one  point  and  it  is  a  bl  polygon.  Q.E.D. 

Theorem  8:  if  IP  is  a  BI  polygon,  then  SP(i)=r(IP). 

Proof:  Since  IP  is  a  BI  polygon,  all  its  bisectors  intersect  at  a 
single  point  0  and  the  distances  from  0  to  each  side  are  all  equal. 
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Refer  to  Fig.  5.16,  which  shows  point  0  and  the  ith  side  of  IP. 

Evidently , 

I  Vi  |  =  a  +  b  =  S  cot(j^)  +  S  cot(9^  — )  (5.33) 

where  S  is  the  distance  from  0  to  side  i.  Substituting  I  Vi |  into  the 
term  of  tneoran  3  reveals  that  it  gives  a  constant  1/S.  Moreover,  since 
S  is  the  same  for  every  side  of  IP,  hence  the  condition  stated  in 
Theorem  3  is  true  and  SP ( 1 )  is  similar  to  IP.  Q.E.O. 

Theorem  9:  if  SIP  is  the  set  of  all  IP  which  have  the  property  of 
SP ( 1 )=r( IP ) ,  then  SIP  =  BIP. 

Proof:  From  Theorem  7,  one  knows  that  if  IPc-SIP  then  IPeBIP,  which 
implies  that  SIPcBIP.  On  the  other  hand,  from  Theorem  8,  if  IP€3IP  then 
IP^SIP,  thus  BIPCSIP.  Therefore  SIP  =  BIP.  Q.E.D. 

Although  BIP  polygons  are  of  theoretical  interest  and  their  OSP  can 
be  easily  computed,  they  are  not  likely  to  occur  in  rough  terrain  loco¬ 
motion.  Thus,  from  practical  point  of  view,  polygons  which  are  not  BIP 
are  more  closely  related  to  vehicle  locomotion.  The  following  analysis 
deals  with  polygons  of  this  class. 

5. 3. 2. 3  Cases  where  SP ( 1 )  Is  Not  Similar  To  IP 
For  these  cases,  the  relation 

N 

uosm  =  I  °k 
k=l 
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still  holds,  however,  the  ratio  between  D|<+i  and  0^  i  s  no  longer  a  con¬ 
stant,  hence  the  summation  can  not  be  expressed  in  the  same  closed  form 
as  in  Eq.  ( 5. 16) . 

L>0Sin  is  optimal  by  the  principle  of  optimality  [60],  which  states 
that,  in  a  multistage  path,  the  optimal  path  is  comprised  by  each  opti¬ 
mal  subpath  between  two  neighboring  stages.  To  relate  the  principle  of 
optimality  to  the  optimal  algorithm  described  earlier,  one  may  represent 
tne  algorithm  as  a  multistage  path,  where  the  starting  point  is  the  IP 
and  SP(i),  SP(2),  ...  are  the  successive  nodes  along  the  path.  More¬ 
over,  there  are  only  two  paths  between  neighboring  nodes,  one  will  lead 
to  the  OSP  while  the  other  will  not.  When  applying  the  algorithm,  it  is 
clear  that  the  region  outside  the  shrunken  polygon  will  not  give  the 
optimal  result,  thus  it  is  excluded  from  further  consideration,  i.e., 
only  tne  SP  is  investigated  in  the  next  iteration.  This  point  is 
illustrated  by  the  example  shown  in  Fig.  5.17  where  tne  IP  is  a  trape¬ 
zoid.  After  finding  the  GC  and  shrinking  it  once,  SP ( 1 )  degenerates 
into  two  triangles.  It  is  apparent  that  Acde  needs  not  be  considered 
any  further  since  every  point  inside  it  gives  a  distance  to  sides  L2  and 
L3  that  is  shorter  than  0.  In  other  words,  all  points  inside  the 
trapezoid  except  Aabc  are  excluded  from  further  consideration,  and  the 
USP  of  Aabc  is  also  the  USP  of  the  initial  trapezoid. 

For  practical  purpose,  the  shrinking  process  may  be  terminated  if  U 
is  smaller  than  a  threshold,  and  the  algorithm  can  be  considered  as 
finite-step  in  this  sense.  At  least,  it  can  be  stated  that  the  algor- 
i  thm  is  "D|<  near  optimal"  or  "optimal  within  D^".  The  threshold  may  be 
set  according  to  tne  accuracy  of  the  vehicle.  For  instance,  the  posi- 
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tion  control  of  the  OSU  Hexapod  has  an  accuracy  of  0.5  inch,  and  this 
value  may  be  selected  as  the  threshold. 


5. 3. 2. 4  Classification  of  Polygons 

Polygons  inay  be  divided  into  the  following  three  catagories  based 
on  the  nature  of  OSP  and  the  maximum  stability  margin. 

(1)  Polygons  whose  OSP  is  unique  and  the  distances  from  the  OSP  to 
all  side  are  the  same.  The  set  of  these  polygons  is  described  earlier 
as  BIP  or  SIP.  Fig.  5.18  shows  a  polygon  which  belongs  to  this  group 
but  is  not  regular.  This  polygon  is  created  as  follows.  First,  the  OSP 
0  is  specified.  Then  the  four  segments  0A,  OB,  OC,  and  00,  which  have 
the  same  length,  are  drawn  in  such  a  way  that  the  angles  between  them 
(angles  1,  2,  3,  and  4)  are  not  all  equal.  Lines  which  are  perpen¬ 
dicular  to  one  of  the  four  segments  are  then  created,  and  the  inter¬ 
sections  of  these  lines  define  the  resultant  polygon. 

(2)  Polygons  whose  OSP  is  not  unique.  An  example  is  the  rectan¬ 
gular  given  in  Fig.  5.12.  As  mentioned  before,  all  points  within  seg¬ 
ment  A8  provide  the  same  optimal  stability  margin  with  respect  to  sides 
L2  and  L4. 

(3)  Polygons  whose  OSP  is  unique  but  the  distances  from  the  OSP  to 
all  sides  are  not  equal.  The  trapezoid  shown  in  Fig.  5.17  is  such  an 
example.  The  OSP  of  this  polygon  is  also  the  OSP  of  Aabc,  and  it  is 
obvious  that  the  distances  from  the  OSP  to  the  four  sides  are  not  the 
same. 
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S.3.2.5  Remarks  about  the  Algorithm 


A  few  remarks  related  to  the  optimal  algorithm  are  given  below. 

(1)  Two  assunptions  were  made  for  the  development  of  the 

algorithm.  First,  the  footholds  of  all  six  legs  were  assumed  to  be 
known  before  the  body  moved.  Second,  in  defining  the  supporting 
polygon  and  the  projection  of  the  center  of  the  body,  it  was  assumed 
that  all  legs  were  massless. 

(2)  No  assumption  was  made  on  the  type  of  gait  nor  the  type  of  loco¬ 
motion.  Thus  it  is  applicable  to  any  kind  of  gait  for  any  type 
of  motion. 

(3)  Since  the  execution  of  the  algorithm  is  rather  time-consuming,  the 
geometric  center  is  used  as  the  destination  of  the  center  of  the 
body  in  the  current  software. 

5. 3. 2. 6  Utilization  of  the  DSP  in  FTL  Motion 

Tne  OSP  concept  is  incorporated  into  the  current  software  to 
realize  the  FTL  operation  in  the  following  way.  First,  the  GC  of  the 
present,  o-vertex  supporting  polygon,  denoted  as  0SP1,  is  computed. 

After  a  foothold  is  assigned  for  leg  1,  the  next,  6-vertex  supporting 
polygon,  which  is  formed  with  leg  1,  3,  and  5  at  their  new  positions 
while  legs  2,  4,  and  b  in  the  old  positions  (Fig.  5.19),  is  projected 
and  its  GC,  denoted  as  0SP2,  is  computed.  Then  during  the  period  of 
body  motion,  the  center  of  the  body  moves  from  0SP1  toward  0SP2,  which 
occurs  after  middle  leg  placement  and  before  rear  leg  placanent.  During 
the  second  half  of  the  locomotion  cycle,  a  foothold  is  assigned  for  leg 
2.  Again,  the  following,  b-vertex  supporting  polygon  with  all  6  legs  in 
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Foothold 


igure  5,19  The  Polygons  Used  to  Determine  the 
Motion  of  the  Body  for  the  First 
Hal f  Cycle. 


Foothol  d 


gure  5,20  The  Polygons  Used  to  Determine  the  Motion 
of  the  Body  for  the  Second  Half  Cycle. 


their  new  locations  (Fig.  5.20)  is  calculated  and  its  GC,  denoted  as 
0SP3,  is  computed.  Then  the  body  will  move  from  QSP2  toward  0SP3.  Note 
that  only  three,  6-vertex  polygons  have  been  taken  into  consideration  in 
the  above  discussion.  However,  in  an  actual  FTL  locomotion,  more  than 
three  supporting  polygons  are  involved.  Furthermore,  some  of  them  are 
5-vertex  polygons  and  the  others  are  o-vertex.  All  the  polygons  which 
show  up,  one  time  or  another,  during  an  entire  FTL  motion  are  given  in 
Fig.  5.21.  There  are  totally  9  different  supporting  polygons  for  a 
complete  locomotion  cycle  provided  that  only  one  leg  moves  at  a  time. 
Fig.  5. 21. a  is  the  initial  configuration  which  shows  both  the  Hexapod 
body  and  the  6-vertex  polygon.  The  numbers  indicate  the  actual  leg 
position  at  that  time.  In  Fig.  5.21.b,  leg  1  is  moving  toward  foothold 
1  tnus  the  polygon  is  a  5-vertex  one.  Whenever  both  leg  1  and  leg  5  are 
in  the  supporting  phase,  the  shape  of  the  polygon  does  not  change  re¬ 
gardless  if  leg  3  is  in  the  air  or  on  the  ground.  This  situation  is 
reflected  in  Fig.  5.21.C,  where  the  missing  number  “3"  implies  the 
"don't  care"  condition  for  leg  3.  When  leg  5  is  moving,  another  5- 
vertex  polygon  appears  as  shown  in  Fig.  5. 21 . d .  The  rest  of  these 
figures  can  be  interpreted  in  a  similar  way.  Comparing  Figs.  5.19, 

5.2U,  and  5.21,  it  is  clear  that  the  present  software  only  considers  3 
of  the  9  polygons,  which  are  (a),  (e),  and  (i).  In  order  to  faithfully 
fulfill  the  concept  of  OSP,  all  polygons  should  be  considered,  their 
OSPs  should  be  computed,  and  the  body  should  react  in  such  a  way  that 
whenever  a  new  polygon  appears,  it  would  move  toward  the  OSP  of  the 
newest  polygon.  By  this  philosophy,  not  only  that  the  body  has  to  move 
more  frequently,  as  opposed  to  the  adopted  scheme  where  the  body  motion 
is  confined  to  two  specific  instants,  but  also  that  the  vehicle  has  to 
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5  3  1 


Figure  5.21  All  Polygons  Associated  with  a  FTL  Locomotion. 


move  back  and  forth  to  accommodate  the  drifting  of  OSPs,  which  may  not 
be  desirable  because  of  the  backward  motion  involved.  To  illustrate, 
the  geometric  center,  the  front  longitudinal  margin,  the  rear  longi¬ 
tudinal  margin,  and  the  resultant  longitudinal  stability  margin  of  each 
of  the  9  polygons  are  calculated  and  listed  in  Table  5.1.  The  GC  is 
used  in  place  of  the  OSP  for  the  simplicity  of  computation.  These 
quantities  are  calculated  as  follows.  First,  the  initial  position  of 
each  leg  and  the  two  footholds  are  assigned  as 

leg  1  =  (  0.3L,  -K), 
leg  2  =  (  G.5L,  K), 

leg  3  =  (  -0.1L,  -K), 

1  eg  4  *  (  0.1L,  K), 

leg  5  =  (  -U.5L,  -K ) , 
leg  o  =  (  -Q.3L,  K), 

FTHU  1  =  (  0.71,  -K), 

FTHL)  2  =  (  0.9L,  K),  (5.34) 

where  L  is  the  body  length  of  the  vehicle  and  is  measured  as  the  di s- 
tance. between  a  front  and  a  rear  legs,  K  is  half  of  the  body  width,  and 
the  origin  is  the  center  of  the  body.  These  positions  are  assigned  in 
this  way  so  that  the  distance  between  the  present  position  and  the 
foothold  of  all  legs  are  equal.  The  quantities  in  Table  5.1  then  can  be 
computed  by  using  the  coordinates  of  those  vertices  which  define  the 
polygon.  For  example,  consider  Fig.  5. 21. a.  This  polygon  is  defined 
by  legs  1,  2,  5,  and  b,  thus  its  GC  is 
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TABLE  5.1 


The  Geometric  Center,  Front  Longitudinal 
Margin,  Rear  Longitudinal  Margin,  and 
Longitudinal  Stability  Margin  of  the  9 
Polygons  Occurring  in  FTL  Locomotion. 

(L  is  the  length  of  the  vehicle.) 


(0.0) 


[:0.1L,0) 

(0.1L,0)_ 

(U.3L.0) 


(0.2L.0) 


(0.1L.0) 


(0.3L.0) 


xgc  =  (U.3L+0. 5L-Q. 5l-j. 3L )/4  =  U 


Ygc  =  (-K+K-K+K  )/4  =  0  (5.35) 

The  front  longitudinal  margin  is  the  longitudinal  distance  from  G.C  to 
the  the  front  boder,  12,  of  the  polygon.  Every  point  lying  on  segment 
12  satisfied  the  following  equation: 


0.5L  -  x  _  K  -  y 
x  -  U.3L  y  +  K 


(5.36) 


The  intersecting  point  of  segment  12  and  the  horizontal  line  wich  passes 
through  the  GC  obviously  has  a  Y  coordinate  of  U,  hence  its  X  coordinate 
can  be  easily  calculated  by  substituting 

y  =  0  (5.37) 

into  Eq.  (5.36),  and  the  result  is 

x  =  0.4L  (5.38) 

Consequently,  the  front  longitudinal  margin  is  0.4L.  The  rear  longi¬ 
tudinal  margin  can  be  found  in  exactly  the  same  way  and  has  a  value  of 
U.4L.  Since  the  front  and  rear  margins  are  equal,  the  longitudinal 
stability  margin  is  also  0.4L.  Similar  computation  can  be  performed  for 
every  polygon  to  obtain  the  numbers  shown  in  Table  5.1. 

Note  that  the  GC  of  each  polygon  is  at  di fferent  location,  there¬ 
fore  the  body  has  to  move  8  times  if  it  is  required  to  move  to  every  new 
GC.  Moreover,  by  looking  into  the  coordinates  of  each  successive  GCs, 
the  direction  of  the  motion  of  the  body  has  the  following  sequence: 
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back,  forth,  forth,  back,  back,  forth,  forth,  and  back.  Also  note  that 
the  minimal  longitudinal  stability  margin  of  the  entire  cycle  is  0.3L. 

It  is  interesting,  at  this  point,  to  compare  the  longitudinal 
stability  margins  exhibited  by  the  following  gaits:  the  optimally 
stable  wave  gait,  the  regular  symmetric  gait  1-3-5-2-4-6  with  constant 
body  velocity,  FTL  gait  using  only  three  polygons,  and  FTL  gait  using  9 
polygons.  The  comparisons  were  presented  in  Table  5.2. 

In  1974,  McGhee  and  Sun  [61]  determined  the  longitudinal  margin  for 
all  regul ar  symmetrical  wave  gaits  for  any  even  number  of  legs.  Ac¬ 
cording  to  their  calculations,  this  quantity  of  a  hexapod  vehicle  is 
0.281  for  a  duty  factor  of  5/6,  and  is  D.35L  for  a  duty  factor  of  1. 

Sun  has  also  computed  the  longitudinal  stability  margin  for  all 
regular  symmetric  gaits  of  a  hexapod  machine  [39].  His  result  indicates 
that  for  the  gait  sequence  1-3-5-2-4-6  and  a  constant  body  velocity,  the 
value  is  0. 1L  for  a  duty  factor  of  5/6,  and  is  0.15L  for  a  duty  factor 
of  1. 

For  FTL  gaits  using  9  polygons,  the  resultant  margin  is  0.3L  as 
illustrated  in  Table  5.1.  If  only  three  polygons  are  considered,  the 
stability  margin  is  0.2L.  In  this  case,  when  polygon  (b)  appears,  the 
body  does  not  move  and  the  center  of  body  remains  at  location  (0,0). 
Recall  from  Table  5.1  that  if  the  body  moves  into  location  (-0.1L,  0), 
the  margin  would  be  0.3L  in  both  directions.  Thus  with  the  body  stays 
at  (0,0),  the  front  margin  becomes  Q.2L  while  the  rear  margin  is  0.4L, 
which  gives  a  stability  margin  of  0.2L. 

To  summarize,  different  motion  algorithms  based  on  the  concept  of 
0SP  can  be  devised,  depending  the  number  of  polygons  that  are  taken  into 
account,  whether  they  are  5-vertex  or  u-vertex  polygons,  and  if  backward 
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TABLE  5.2 


Comparisons  of  Different  Gaits  in  Terms 
of  the  Longitudinal  Stability  Margin. 

(3  is  the  duty  factor  and  L  is  the 
length  of  the  vehicle.) 


Gait 

Longitudinal  Stability  Margin 
(3  =  5/6)  (3=1)' 

•  Optimally 

Stable  Wave 

Gai  ts 

Q.23L  0.35L 

Regul ar, 

Symmetrical 

Gait  (1-3-5- 
2-4-6)  with 
Constant  Body 
Velocity 

0.1L  0.1 5L 

FTL  Gait 

Using  3 

Polygons 

0.2L 

FTL  Gait 

Using  9 

Polygons 

0.3L 

194 


i 


! 


w 


motions  are  allowed.  It  should  be  emphasized  again  that  the  present 
software  adopts  a  scheme  which  only  employs  three  specific  6-vertex 
polygons  in  navigating  the  vehicle.  This  particular  scheme  indeed 
proves  the  feasibility  of  the  FTL  operation  and  was  successfully  imple¬ 
mented.  However,  it  is  by  no  means  the  only  solution  since  many  other 
motion  algorithms  are  possible  as  explained  above.  As  such,  the  intro¬ 
duction  of  the  uSP  concept  stimulates  many  interesting  research  issues 
in  vehicle  navigation  which  remain  to  be  answered. 

5.3.3  Heading  Ui rection--What  is  the  Turning  Angle 

When  the  center  of  the  body  is  moving  toward  a  new  location  as 
determined  in  the  previous  section,  it  can  assume  either  a  pure  turning 
mode,  a  pure  crabbing  mode,  or  a  combination  of  these  two  modes  [5]. 
General ly  speaking,  the  motion  of  body  would  involve  X,  Y  velocities  and 
a  turning  rate.  While  the  linear  velocities  are  easily  computed  by 
dividing  the  displacement  vector  with  its  traveling  time,  the  deter¬ 
mination  of  trie  turning  rate  requires  some  more  thought.  One  approach 
is  to  store  all  the  previous  locations  that  the  center  of  the  body  has 
traversed  and  to  fit  them  by  a  polynomial  so  that  the  next  movement  can 
also  be  described  by  this  polynomial.  It  takes  large  amount  of  memory 
space  to  store  the  trajectory  and  requires  a  great  deal  of  computing 
time  to  update  all  points  as  the  body  moves  around.  Closer  investi¬ 
gation  indicates  that  it  is  not  necessary  to  record  all  the  previous 
positions  since  not  all  of  them  have  the  same  effect  on  the  upcoming 
movement.  Fig.  5. 22. a  shows  a  motion  trajectory  which  is  varying 
constantly  and  the  local  behavior  is  quite  different  from  the  global 
pattern.  It  is  obvious  that  local  points  have  more  influence  in  de- 
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ciding  the  next  action  and  are  thus  more  important  than  other  points. 

In  fig.  5. 22. b ,  the  local  behavior  agrees  with  the  global  one  hence  it 
is  sufficient  to  use  only  recent  points  to  prepare  for  next  movement. 
Thus  it  was  decided  in  this  research  to  utilize  three  local  points  to 
specify  the  turning  angle.  They  are:  the  next  OSP,  the  present  OSP, 
and  OSP  of  the  last  cycle.  The  turning  angle  is  computed  as  the  inter¬ 
secting  angle  between  vectors  VI  and  V2  as  shown  in  Fig.  5.23.  In 
practice,  a  turning  angle  is  not  allowed  to  exceed  a  maximum  value  to 
avoid  leg  interference.  If  the  computed  value  is  larger  than  the 
acceptable  one,  it  will  only  turn  as  much  as  it  is  permitted  and  try  to 
make  up  the  difference  during  the  next  coming  cycle. 

5.4  Checking  for  Kinanatic  Margin 

During  motion,  if  any  leg  goes  out  of  its  kinematic  limits,  the 
power  of  all  joint  motors  must  be  turned  off  to  avoid  damage  to  the 
Hexapod,  and  the  experiment  is  terminated  abruptly.  This  constituted 
much  of  the  author's  frustration  in  performing  experiments  because  (1) 
the  phenomena  that  are  of  interest  and  are  expected  to  be  the  main  ob¬ 
servations  may  be  in  the  latter  part  of  a  locomotion,  and  (2)  if  Hexapod 
is  unable  to  recover  from  an  undesired  posture  through  normal ization  or 
a  manually  operated  battery  box,  it  requires  several  persons  to  lift  and 
move  it  by  hand.  With  this  concern,  it  is  highly  desirable  to  be  able 
to  check  beforehand  the  joint  angle  that  every  joint  may  possess 
throughout  the  next  cycle  and  report  all  cases  that  violate  the 
specified  limits.  The  mechanical  limits  of  the  joints  are  as  follows: 
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(a)  (b) 


Figure  5.22  Possible  Trajectories  of  the  Center  of  the  Body. 

(a)  Global  Pattern  Differs  from  Local  Pattern; 

(b)  Global  Pattern  Agrees  with  Local  Pattern. 


1 


Figure  5.23  Determination  of  the  Turning  Angle. 
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lot  <  75° 

where  e \  is  the  angle  of  a  elevation  joint,  92  is  the  angle  of  a  knee 
joint,  and  $  is  the  angle  associated  with  an  azimuth  joint. 

There  is  a  one-to-one  correspondence  between  the  three  joint  angles 
of  each  leg  and  the  linear  coordinates  (x,y,z)  of  the  foot  tip.  Their 
relationship  are  expressed  as  a  set  of  kinematic  equations  and  inverse 
kinematic  equations  [56].  Once  the  next  foothold  is  known,  the  corres¬ 
ponding  joint  angles  are  computed  to  see  if  any  of  them  is  out  of 
bounds.  Since  footholds  are  represented  in  the  body  coordinate  system, 
checking  for  kinematic  margin  during  a  cycle  has  to  be  carried  out  when¬ 
ever  the  body  moves  into  a  new  position.  For  the  sequence  in  Fig.  5.3, 
checking  would  be  performed  at  three  occasions  in  a  cycle  as  follows: 

(1)  The  first  checking  is  due  upon  the  acquisition  of  a  new  foot¬ 
hold  and  before  one  of  the  front  feet  takes  off.  This  allows  the 
Hexapod  to  know  whether  a  newly  specified  foothold  is  reachable  within 
one  step.  If  it  it  is  too  far  away,  the  Hexapod  would  prompt  the  oper¬ 
ator  to  assign  another  one.  Note  that  this  checking  is  needed  not 
because  the  body  has  moved,  but  because  a  new  foothold  is  given.  Hence, 
only  leg  I  or  leg  2  needs  to  be  checked,  depending  on  which  leg  the 
foothold  is  for. 

(2)  The  second  checking  is  associated  with  the  body  motion  after 
leg  3  is  transferred  to  a  new  location.  Here,  all  six  legs  are  involved 
in  checking  since  coordinates  of  their  foot  tips  all  change  as  the  body 
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moves.  By  the  optimal  algorithm  described  earlier,  the  new  location  of 
the  center  of  oody  can  be  computed,  and  each  foot  tip  position  after  the 
body  moves  can  be  calculated  in  advance  to  foresee  any  abnormality. 

(3)  The  third  checking  is  similar  to  the  second  but  is  linked  to 
tne  body  motion  after  the  action  of  leg  4. 

In  programming,  all  three  checkings  are  "simulated"  before  real 
movement  is  initiated.  If  any  violation  is  reported,  the  message  "OUT 
OF  REACH,  TRY  AGAIN  !"  will  be  displayed  on  the  screen  of  a  terminal  to 
ask  tne  operator  to  select  another  foothold.  It  should  be  emphasized 
again  that  this  is  possible  because  the  trajectory  of  the  center  of  body 
throughout  a  cycle  can  be  predicted  beforehand. 

5.5  Update  Coordinates  of  Footholds  as  3ody  Moves 

Every  foothold  specified  by  the  operator  is  used  by  all  three  legs 
on  tne  same  side  in  three  consecutive  cycles.  This  introduces  the 
necessity  to  store  and  update  coordinates  of  footholds  for  at  least 
three  cycles.  Two  different  ways  of  dealing  with  this  problem  are  de- 
1  ineated  below. 

(1)  The  open-loop  approach.  In  this  case,  each  foothold  is  treated 
in  exactly  the  same  way  as  a  supporting  leg  is  handled  in  terms  of  the 
computation  of  new  coordinates  as  seen  from  the  body  coordinate  system. 
Since  the  velocities  and  turning  rate  are  known,  the  calculation  may  be 
done  based  on  the  "relative  motion"  concept.  Mathematical  analysis  of 
this  part  was  provided  by  Wahawisan  in  [56].  This  method  is  considered 
to  be  an  open-loop  way  because  all  computations  utilize  commanded  body 
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velocities  and  turning  rate  as  the  primary  parameters  without  verifying 
if  those  commanded  values  have  really  been  reached.  If  the  body  follows 
them  closely,  a  good  result  is  obtained;  otherwise,  errors  in  foothold 
coordinates  tend  to  accumulate  from  cycle  to  cycle. 

(2)  The  close-loop  approach.  In  contrast  to  method  (1),  actual 
oody  motion  instead  of  commanded  body  motion  is  used  here  to  update 
footholds.  Actual  body  motion  on  even  terrain  is  characterized  by  (dx, 
dy ,  0),  where  dx  and  dy  are  the  displacement  components  in  X  and  Y 
directions  and  0  is  the  turning  angle.  These  quantities  are  measured  at 
the  new  body  position  with  respect  to  the  old  body  coordinate  system,  as 
illustrated  in  Fig.  5.24.  Values  of  dx,  dy  and  are  computed  by  using 
the  actual  foot  tip  positions  of  two  supporting  legs  at  instants  tl  and 
t2,  where  tl  corresponds  to  the  time  when  body  assuned  the  old  position 
and  t2  is  associated  with  the  new  location.  The  following  derivation 
shows  how  they  are  calculated. 

Suppose  that  during  the  time  interval  [tl,t2]  the  center  of  body 
moved  (dx,dy)  and  turned  an  angle  of  9 .  The  geometrical  relationship 
between  the  new  and  old  coordinate  systems  may  be  expressed  as  a  HTM  °T^ 
where 


°Tn  = 


cose 
si  ne 
0 
0 


-sine  0 

cose  0 

0  l 

G  0 


dx 

0 

1 


(5.39) 


The  coordinates  of  a  foothold  in  both  OS's  are  related  by  °T^  in  the 
fol I owi ng  way: 
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Foothold 


Figure  5.24  Foothold  Locations  as  Seen  from  the  Old 
and  the  New  Body  Positions, 
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where  the  superscript  * n 1  and  'o'  denote  new  and  old  OS’s,  and  the  sub¬ 
script  i  indicates  it  is  the  i th  leg. 

Performing  multiplication  on  the  right  hand  side  and  equating  the 
corresponding  row,  two  equations  are  obtained: 


=  cose  • 


=  sine  • 


t*  - 

i 


VN 

X  4 
1 


si  ne 


cose 


Y.  +  dx 

l 


YN  +  dy 


(5.41) 


Therefore,  during  that  period,  if  Doth  leg  1  and  leg  2  are  in  the 
supporting  phase,  then  four  equations  are  available: 


o  N  N 

=  cose  •  -  sine  •  Yj,  +  dx, 


o  N  N 

Y^  =  si.no  •  X  +  cose  •  Y  +  dy 


0  N  N 

=  cose  •  X  -  sine  •  Y2  +  dx 


vo  .  .M  . 

X  =  sm  •  X^  +  sine  •  Y2  +  dy 


(5.42) 


ComDining  the  similar  terms  in  Eq.  (5.42)  results  in 
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(X®  -  X°)  =  cosa(X^  -  X?J)  -  sine  (Y^J  -  yJJ)  (5.43) 

(Y®  -  Y®)  =  sine  (X^  -  X*)  +  cose  ( Y1^  -  Y^)  (5.44) 

Multiplying  Eq.  (5.43)  wi  tn  cose  and  Eq.  (5.44)  with  sine  yields 

cose  (X®  -  x®)  «  cosZetxJ  -  x'g)  -  cose  •  sine (Y^  -  yJ) 

sine  (Y®  -  Y°)  =  sin2e(x'1J  -  x'j)  +  sine  •  cose(Y^  -  yJ) 

(5.45) 

Adding  them  together  leads  to 

(X®  -  X®)  •  COS0+  (Y®  -  Y®)  •  sine  =  (xj  -  X^)  (5.46) 


Let 


A  =  <xj  -  X*) 


*  -  X2} 


=  (Y®  -  Y®) 


(5.47) 


Substituting  them  into  Eq.  (5.46)  gives 

3  .  cose  +  C  .  sing  =  A  (5.48) 

By  taking  out  a  common  factor,  Eq.  (5.48)  can  be  rewritten  as 

_  Q  C 

/d2  +  c2  .  (  — - —  cose  +  — ~  sine)  =  a 

/82  +  C2~  /B2  +  W 

(5.49) 
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I 


Let 


si  n<t) 


cos<j> 


Eq.  (5.49)  becomes 


sin#  cose  +  coso 


Hence 

sin(o  +  o )  =  K 

O+o  =  sin-1  (K) 
Si  nee  tari}>  =  3/C 

thus, 

0  =  tan'l  (3/C) 


K 


(5.50) 

(5.51) 

(5.52) 

(5.53) 

(5.54) 


Therefore, 

9  =  sin'l-(K)  -tan-1(B/C)  (5.55) 

The  values  of  dx  and  dy  then  can  be  computed  by  using  Eq.  (5.42). 


5.6  Contact  Sensing 

When  a  leg  is  placed  down  at  the  end  of  a  transfer  phase,  it  may 
fail  to  reach  the  terrain  surface  because  (1)  the  foothold  position 
provided  by  the  vision  system  has  a  significant  error  in  the  Z  direc¬ 
tion,  or  (2)  tne  servo  loop  fails  to  produce  the  commanded  movement. 
This  may  introduce  undesirable  consequence  for  the  Hexapod.  For 
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instance,  considering  the  FH  sequence,  leg  3  is  supposed  to  take  off  as 
soon  as  leg  1  touches  down.  If  leg  1  does  not  make  contact  with  the 
ground  while  leg  3  lifts  up  as  scheduled,  then  on  the  left  side  of  the 
Hexapod  only  leg  5  is  actually  in  the  supporting  phase  and  shares  the 
weight  of  the  vehicle.  Leg  5  would  be  overloaded  and  the  machine  be¬ 
comes  unbalanced.  Therefore,  it  is  very  desirable  to  be  able  to 
guarantee  that  a  leg  truly  reaches  ground  when  it  is  coming  down.  With 
the  force  sensor  installed  inside  each  leg,  this  can  be  accomplished  by 
feeding  back  the  reacting  force  and  checking  if  its  value  exceeds  a 
threshold,  say,  15  pounds.  In  cases  tnat  a  transfer  phase  expires  and 
the  reaction  force  is  still  smaller  than  the  threshold,  the  leg  will  be 
commanded  to  extend  its  motion  in  the  Z  direction;  meanwhile  the  leg 
scheduled  to  lift  up  next  will  be  delayed  until  the  force  sensor  indi¬ 
cates  that  contact  has  been  made  for  the  preceding  leg.  This  contact 
testing  has  been  included  in  the  author's  software  WALK37  (see 
Appendix) . 

Another  possible  application  of  force  sensors,  which  remains  to  be 
implemented,  is  to  determine  if  the  terrain  surface  is  stiff  enough  to 
support  the  load  of  a  leg.  For  example,  if  a  foothold  is  assigned  on 
the  surface  of  quicksand,  force  fed  back  from  the  leg  should  indicate, 
to  some  degree,  the  texture  of  that  footnold.  If  no  substantial  force 
is  sensed  after  a  period  of  searching,  it  would  be  concluded  that  the 
point  is  not  solid  enough  for  a  leg  to  step  on  and  another  one  should  be 
selected. 

In  addition,  a  force  sensor  can  be  used  to  realize  active  compli¬ 
ance  [62].  Tnis  subject  is  covered  in  Section  6.3. 
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5.7  Overview  of  FTL  Locomotion 


The  motion  algorithm  described  in  this  chapter  is  summarized  into 
the  flowchart  shown  in  Fig.  5.25.  The  operator  initializes  the  first 
half  notion  cycle  by  assigning  a  foothold  for  leg  1.  This  foothold  is 
perceived  by  the  vision  system  and  its  coordinates  are  computed  in  the 
11/7U  computer  using  tri angulation.  With  this  information,  the  next, 
o-vertex  supporting  polygon  can  be  identified,  its  optimally  stable 
point  can  be  computed,  and  the  motion  of  the  body  can  be  pre-planned.  A 
checking  procedure  is  then  invoked  to  simulate  the  planned  motion  and  to 
see  if  any  leg  would  go  out  of  its  kinematic  limits.  The  foothold  would 
be  rejected  for  any  violation  of  kinematic  margin  and  the  operator  would 
be  asked  to  assign  another  point.  After  a  foothold  is  accepted,  leg 
motion  begins.  Leg  1  moves  first,  followed  by  leg  3,  then  the  body 
moves,  and  then  leg  5  is  moved.  These  events  constitute  the  activities 
of  the  first  half  cycle.  The  scenerio  of  the  second  half  cycle  is 
exactly  the  same  except  that  it  involves  legs  2,  4,  and  6  instead  of  1, 
3,  and  5.  A  new  cycle  then  starts  and  the  whole  operation  is  repeated. 

5.8  Summary 

Algorithms  which  have  been  developed  to  facilitate  FTL  motion  were 
presented  in  this  chapter.  Activities  that  occur  at  the  locomotion- 
cycle  level  were  discussed  in  detail.  A  particular  gait  sequence,  1- 
3-5-2-4-6,  was  selected  to  implement  FTL  operation  because  it  satisfies 
the  requirement  of  FTL  motion  and  possesses  the  property  of  physical 
symmetry.  The  behavior  of  the  body  was  investigated  with  respect  to 
timing,  destination,  and  heading  direction.  The  body  was  scheduled  to 
move  after  the  placement  of  the  middle  leg  and  before  the  placement  of 
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Figure  5.25  Flowchart  of  FTL  Walking  Algorithms. 
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a  rear  leg.  All  six  legs  are  in  the  supporting  phase  during  body  motion 
to  enhance  stability.  The  destination  of  the  body  was  chosen  as  the  the 
OSP  of  the  next,  6-vertex  supporting  polygon,  and  only  three  such  poly¬ 
gons  were  considered  in  the  current  software.  The  heading  direction  was 
detennined  bu  using  Three  consecutive  OSPs.  It  should  be  point  out  that 
the  geometric  center  of  all  six  legs  is  used  in  place  of  the  OSP  for 
consideration  of  real-time  programmi ng. 

A  heuristic  algorithm  was  presented  to  find  the  OSP  of  an  arbitrary 
polygon.  For  simple  polygons  such  as  regular  polygons  or  BIP,  the  al¬ 
gorithm  leads  to  the  same  result  as  produced  by  the  geometrical 
approach,  which  locates  the  OSP  at  the  intersection  of  all  angular 
bisectors.  In  general,  the  shrinking  operation  has  to  be  applied 
iteratively  to  identify  OSP.  This  process  is  rather  complicated  and 
time-consumi ng,  thus  is  not  included  in  the  present  version  of  software 
( tne  geometric  center  is  used  instead  of  the  OSP).  However,  recent 
study  by  Miller  [63]  indicates  that  array  processors  can  efficiently 
perform  "image  shrinking" ,  which  has  a  similar  geometric  nature  to  the 
shrinking  of  a  polygon.  Hence,  the  computation  of  OSPs  may  become 
feasible  for  real-time  control  if  array  processors  are  employed. 

Through  the  realization  of  FTL  motion,  it  is  also  recognized  tnat 
what  is  real ly  needed  is  a  body  servo  planning  routine  which  navigates 
from  one  OSP  to  the  next  for  every  supporting  pattern  while  maximizing 
stability  margin  over  the  entire  trajectory  and  rotating  the  body  to 
maximize  kinematic  margin.  Compared  with  this  extremely  complicated  and 
general  problem,  this  chapter  deals  a  much  simplified  case  in  a  parti¬ 
cular  way.  More  research  effort  is  needed  in  this  area. 
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Chapter  6 


ROUGH  TERRAIN  LOCOMOTION 


6.1  Introduction 

Legged  vehicles  are  designed  to  walk  over  rough  terrain;  the 
Hexapod  is  no  exception.  When  moving  on  an  even  surface,  it  may  be 
assigned  that  the  surface  condition  does  not  change,  that  every  point  on 
tiie  ground  is  a  good  foothold,  and  that  footholds  for  all  legs  lie  on 
the  same  plane.  For  such  a  simple  case,  there  is  little  need  for  the 
Hexapod  to  sense  and  analyze  the  terrain;  all  it  has  to  do  is  to 
recognize  and  to  locate  those  specified  footholds.  In  fact,  even  the  Z 
coordinate  of  a  foothold  is  redundant  since  the  Hexapod  knows  that  it 
should  lie  on  the  horizontal  surface.  However,  traversing  rough  terrain 
is  a  different  matter.  Rough  terrain  may  be  defined  as  a  three- 
dimensional  space  in  which  the  heights  (or  Z  coordinates)  of  surface 
points  are  not  constant.  Therefore,  slopes,  ditches,  holes,  obstacles, 
etc.,  may  exist.  Obviously,  the  Hexapod  needs  more  external  sensors  to 
overcome  such  a  complicated  environment.  This  is  why  it  was  restrained 
to  walk  only  on  even  terrain  until  force  sensors  and  a  vertical  gyro 
were  installed.  In  recent  experiments,  Pugh  demonstrated  that,  with  the 
assistance  of  these  devices,  the  Hexapod  was  able  to  stably  traverse  a 
pile  of  wooden  logs  while  maintaining  level  body  motion  [46].  Two 
things  should  be  pointed  out  in  regard  to  his  work.  First,  it  was 
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accomplished  before  the  vision  system  was  constructed,  and  proved  that 
rough  terrain  locomotion  was  possible  even  without  the  vision  function. 
Second,  since  vision  was  not  involved,  no  prior  information  about  the 
terrain  lying  ahead  was  available,  and  everything  was  treated  in  a 
feedback-and-correct  manner,  i.e.;  adjustments  were  performed  only  after 
something  had  happened. 

With  a  vision  system,  the  Hexapod  knows  where  the  next  foothold  is 
before  it  makes  the  subsequent  movement.  This  allows  it  to  prepare  and 
to  make  proper  adjustment  to  accommodate  the  variation  of  terrain  sur¬ 
face.  Ideally  speaking,  if  the  vision  system  is  perfect,  then  the 
Hexapod  should  be  able  to  regulate  itself  adequately  even  without  force 
sensors  or  gyros.  Unfortunately,  like  every  real  system,  the  vision 
system  indeed  has  a  certain  amount  of  error.  Hence,  to  ensure  safety 
and  a  smooth  ride,  algorithms  developed  by  Pugh  are  incorporated  with 
the  vision  system  in  resolving  rough  terrain  locomotion. 

Since  it  is  intended  to  encompass  rough  terrain  locomotion  by 
extending  those  algorithms  described  in  Chapter  5  which  were  developed 
under  the  assunption  that  the  Hexapod  body  is  parallel  to  a  horizontal 
plane,  one  of  the  central  problems  in  dealing  with  uneven  terrain  is  to 
control  its  attitude  so  that  the  boc(y  is  maintained  level.  A  detailed 
treatment  of  this  issue  has  been  given  in  [64]  and  [65].  Thus  only  a 
qualitative  summary  is  presented  in  Section  6.2.  Active  compliance  is 
especially  important  for  rough  terrain  motion  because  it  effectively 
provide  an  adjustable  suspension  system  and  can  reduce  much  of  the 
transient  impact  when  something  unexpected  occurs.  The  theoretical 
oackground  and  mathematical  derivation  of  this  subject  was  delineated  in 
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[65].  Section  6.3  of  this  chapter  attempts  to  give  a  qualitative  in¬ 
terpretation  of  the  effect  of  active  compliance.  Section  6.4  discusses 
the  subject  of  altitude  estimation  which  is  needed  to  update  the  Z 
coordinate  of  footholds  for  rough  terrain  locomotion. 

6.2  Attitude  Control 

The  OSU  Hexapod  is  a  very  complex  machine.  While  the  body  alone 
has  six  degrees  of  freedom  in  motion  (three  in  translation  and  three  in 
rotation),  each  of  the  six  legs  can  move  independently  in  three  direc¬ 
tions,  thus  adding  another  three  degrees  of  freedom  to  the  overall 
motion  of  the  entire  system.  Therefore,  the  total  number  of  generalized 
coordinates  for  a  rigid  body  model  is  24,  impling  a  state  vector  of 
dimension  48  when  position  and  velocity  are  considered  [64].  Previous 
studies  have  shown  that,  for  such  a  complicated  system,  an  analytical 
approach  for  regulation  of  the  movements  is  not  feasible,  even  if  small 
motion  1 inearization  is  assumed  [66].  Hence,  tne  control  of  body  atti¬ 
tude  discussed  here  is  based  on  a  much  simplified  model  which  treats 
individual  supporting  legs  separately.  Precisely,  attitude  control  is 
achieved  through  adjustment  of  the  effective  length  of  all  supporting 
legs,  as  illustrated  in  Fig.  6.1  and  Fig.  6.2. 

Fig.  6.1  shows  the  side  view  of  the  situation  where  the  Hexapod 
has  a  pitch  angle  of  P  radians.  If  the  length  of  leg  F  (front  leg) 
is  decreased  by  azp  and  that  of  leg  R  (rear  leg)  is  increased  by  azr,  as 
seen  in  the  figure,  the  body  would  become  level.  By  simple  trigono¬ 
metry, 

A zp  =  -Xp  •  Tan  P  (6.1) 


211 


where  Xp  is  the  x  coordinate  of  the  foot  tip  of  leg  F  as  expressed  in 
the  body  coordinate  systen.  A  positive  value  of  Azp  indicates  that  the 
length  of  this  leg  should  be  increased  rather  than  decreased. 

Assume  that  attitude  control  is  efficient  so  that  the  pitch  angle 
is  always  Kept  within  a  small  value,  then  tan  P  approaches  P  and 

AzF  =  -XF  .  P  (6.2) 

For  the  rear  leg,  a  similar  analysis  leads  to 

Azp  =  -Xa  •  P  (6.3) 

In  general ,  the  amount  of  adjustment  of  leg  length  az,  of  leg  i  is 

AZi  =  -x-j  .  P  (6.4) 

Note  that  the  sign  of  az-,-  is  decided  by  that  of  Xj  and  the  pitch  angle 
P,  which  is  positive  in  the  counterclockwise  direction  about  the  Y  axis. 
For  instance,  in  Fig.  6.1,  P  is  positive,  Xp  is  positive  but  X^  is  nega¬ 
tive,  therefore  dzp  is  negative  and  dZR  is  positive. 

The  correction  of  leg  length  needed  to  overcome  a  roll  angle  R  can 
be  derived  in  a  similar  way,  and  the  result  is 

AZ-j  =  Y-|«R  (6.5) 

where  Yj  is  the  y  coordinate  of  the  foot  tip  of  leg  i  expressed  in  the 
body  coordinate  system. 

For  s*na!  1  pitch  and  roll  angles,  the  foot  adjustments  needed  for  an 
error  around  a  given  axis  is  independent  of  the  error  around  the  other. 
Thus,  Eqs.  (6.4)  and  ( o . 5)  can  be  combined  together  to  deal  with  the 
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cases  where  both  pitch  and  roll  angles  are  present.  The  result  is 
simply 

A  Zj  =  -Xj  •  P  +  Yj  .  R  (6.6) 

In  additional  to  adjusting  the  Z  position,  the  rate  in  the  Z  • 
direction,  Z,  also  has  to  be  modified  because  a  resolved  rate  control 
mechanism  [44]  is  adopted  for  the  servo  control  loop.  This  quantity  is 
obtained  by  di fferenti ati ng  a Zj  of  Eq.  (6.6)  with  respect  to  time,  which 
gives 

(A z-i  )  =  -Xj  .  P  +  Yj  .  R  (6.7) 

If  one  allows  the  pitch  and  roll  angles  to  decay  exponentially 
rather  than  trying  to  force  them  to  zero  immediately,  which  is  virtually 
impossible  for  a  physical  system,  the  following  relations  hold: 

P  =  -K  .  P 

R  =  -K  .  R  ,  K  >  0  (6.8) 

Substituting  Eq.  (6.8)  into  Eq.  (6.7)  yields 

(a  z-j  )  =  -K(-Xj  .  P  +  Y  .  R)  (6.9) 

and  AZi  can  be  computed  in  real  time  by  integrating  (AZj)  over  a 
time  interval,  giving 

t 

AZi  s  /  (AZj)  •  At  (6.10) 

ts 

where  ts  is  the  instant  at  which  leg  i  enters  the  support  phase. 
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6.3  Active  Compliance 

As  mentioned  in  Section  5.6,  a  force  sensor  can  be  used  as  a  con¬ 
tact  sensing  device  to  assure  that  a  leg  actually  reaches  the  ground 
when  it  is  placed  down.  Another  important  application  of  a  force  sensor 
is  to  implement  active  compliance.  This  is  made  possible  by  feeding 
back  actual  position,  actual  rate,  and  actual  ground  reacting  force  into 
a  rate  control  loop,  as  described  in  the  following  equation  [62]: 

=  ZA  +  Kp  (ZD  '  ZA^  +  MfA  ‘  FU>  (5.12) 

Tnose  variables  with  subscript  "D"  denote  desired  values  and  those  with 
subscript  "A"  represent  actual  values. 

By  assuming  an  ideal  rate  servo  loop  (the  case  of  a  nonideal  loop 
was  discussed  in  [46]),  i.e.,  Za  =  Zq.  Ed-  (6.12)  becomes 


(FA  -  Fq)  =  -Kp/Kf  .  (Zu  -  ZA)  =  K  .  (ZD  -  ZA),  K  >  0  (6.13) 


Note  that  Eq.  (6.13)  may  be  interpreted  as  the  equation  of  a  spring, 
with  K  as  the  spring  constant,  Zq  the  length  of  the  relaxed  spring,  FA 
the  external  compressing  force,  and  the  external  stretching  force. 

Therefore,  with  active  compliance,  a  leg  would  behave  as  i f  a 
mechanical  spring  with  an  adjustable  center  were  built  inside  it.  This 
i nterpretati on  leads  to  the  model  shown  in  Fig.  6.3,  which  provides  a 
good  insight  into  the  nature  of  active  compliance  and  makes  the  makes 
the  explanation  of  its  effect  easier.  Here,  a  leg  is  modeled  as  con¬ 
sisting  of  a  spring  and  a  rigid  segment,  which  means  that  forces  acting 
on  the  foot  tip  will  be  transferred  to  the  spring.  When  a  leg  contacts 
the  ground,  a  compressing  force  FA  is  exerted  at  the  foot  tip.  The 
stretching  force  Fq  is  imaginary  and  is  generated  by  programming.  If 
the  magnitude  of  FA  is  greater  than  that  of  Fq,  the  spring  is  compressed 
and  the  leg  becomes  shorter.  On  the  other  hand,  if  Fq  is  greater  than 
Fa,  tne  spring  is  stretched  and  the  leg  becomes  longer. 

The  merits  of  active  compliance  can  be  fully  appreciated  con¬ 
sidering  the  following  two  cases: 

(1)  Suppose  that  at  the  end  of  a  transfer  phase,  a  leg  does  not 
reach  tne  ground  due  to  errors  or  irregular  terrain  (say,  a  hole). 
However,  during  the  following  motion  planning  cycle,  it  is  treated  as  a 
leg  in  the  supporting  phase  and  is  required  to  share  part  of  the  weight 
of  the  Hexapod.  Thus,  a  nonzero  Fq  is  assigned  by  the  program  to  this 
leg.  Since  it  does  not  make  contact  with  the  ground,  the  value  of  FA  is 
zero.  Consequently,  Fq  is  greater  than  FA  and  the  leg  will  be  extended 
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further  until  it  finally  touches  the  ground,  or  the  the  term  from  posi¬ 
tion  error  and  that  from  force  error  cancel  out.  If  the  hole  is  not  too 
deep,  this  can  ensure  that  a  supporting  leg  always  firmly  contacts  with 
the  ground. 

(2)  Imagine  that  a  leg  is  moving  downward  during  a  transfer  phase 
and  an  obstacle  is  encountered  before  time  expired.  Since,  according  to 
the  timing,  the  leg  is  considered  as  in  the  transfer  phase,  a  zero 
valued  Fy  is  assigned  by  the  program.  Meanwhile,  a  compressing  force  is 
exerted  on  the  foot  tip,  causing  to  be  greater  than  Fy.  This  in  turn 
will  reduce  the  effective  length  of  the  leg.  Notice  that  if  it  were  not 
for  the  active  compliance,  the  leg  would  have  the  same  length  as  other 
legs,  which  would  tilt  the  body  excessively  as  shown  in  Fig.  6.4. 

6.4  Altitude  Estimation 

When  the  Hexapod  is  walking  on  even  terrain  and  the  length  of  all 
legs  is  commanded  to  have  the  same  value  throughout  the  motion,  it  is 
reasonable  to  assune  that  the  height  of  the  body,  which  is  measured 
from  the  center  of  the  body  to  the  terrain  surface,  will  not  change 
signi ficantly .  However,  as  the  terrain  surface  fluctuates  from  point  to 
point  and  the  attitude  control  algorithm  is  imposed  to  keep  the  body 
level,  the  effective  length  of  each  leg  is  subject  to  change.  It  is 
conceivable  that  the  altitude  of  the  body  would  vary  constantly  as  the 
Hexapod  tries  to  accommodate  itself  to  rough  terrain. 

Altitude  estimation  is  needed  due  to  the  necessity  of  updating 
every  foothold  for  at  least  three  cycles.  The  analysis  in  Section  5.5 
deals  with  the  cases  where  the  height  of  the  body  is  invariant  and  thus 
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the  Issue  reduces  to  a  two-dimensional  problem,  i.e.,  updating  the  X  and 
Y  coordinates  of  each  foothold.  Now  that  the  altitude  may  change  from 
time  to  time,  it  is  also  necessary  to  update  the  Z  component  as  well. 
However,  so  long  as  the  Hexapod  is  maintained  level,  the  updating  of  the 
Z  coordinate  is  independent  of  that  of  X  and  Y  components;  hence  the 
result  from  section  5.5  is  still  applicable  and  the  only  additional  work 
is  to  recompute  the  Z  coordinate  of  each  foothold.  Since  the  Z  coor¬ 
dinate  is  expressed  in  the  body  coordinate  system  witn  the  center  of  tne 
body  as  the  origin,  a  change  in  body  altitude  implies  a  variation  in  the 
Z  component  of  every  foothold.  Thus  updating  the  Z  coordinate  is 
essentially  the  same  as  estimating  the  altitude  of  the  body.  For  rough 
terrain  locomotion,  the  Z  coordinate  of  the  foot  tip  of  each  leg  is 
generally  different  from  one  another,  and  the  altitude  of  the  body  can 
be  defined  as  the  average  value  of  the  Z  components  of  all  supporting 
legs.  Hence,  during  the  time  period  [tl,t2],  the  height  of  the  body  at 
tl,  H[tl] ,  is  computed  as 

X 

l  zCi.til 

H[tl]  =  ^ - - -  (5.14) 

where  N  is  the  number  of  supporting  legs  at  tj.  Similarly,  HCt-2 3  is 
calculated  as 


H 

l  Z[i,t2] 

H[t2J  =  -  (6.15) 
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and  the  Z  component  of  a  foothold  at  t^  and  t£  are  related  by  the 
following  equation: 

FTZ [ 1 , t2  3  =  PTZ [ i , 1 1 ]  +  ( H [ t2  3-H [ t ^  3 )  (6.15) 

where  FTZCi * t£^  is  the  Z  coordinate  of  leg  i  at  tl  and  FTZCi.t?]  is  that 
of  the  same  leg  at  t2 .  Therefore,  knowing  FTZ [ i , t ^ ]  and  the  body  alti¬ 
tude  at  ti  and  t2,  FTZ[i,t2]  can  be  computed. 

6.5  Summary 

The  following  modifications  were  made  to  algorithms  developed  in 
Chapter  5  to  deal  with  rough  terrain  locomotion:  (1)  foot  lift  was  in¬ 
creased  from  4  inches  to  10  inches  so  that  a  leg  could  transfer  across 
obstacles  without  collision,  (2)  body  attitude  control  was  imposed  to 
<eep  the  body  level  to  a  horizontal  plane  so  that  motion  in  XY  plane  and 
that  in  Z  direction  were  decoupled,  (3)  active  compliance  was  added  to 
provide  a  suspension  system  so  that  transient  impact  was  minimized,  and 
(4)  altitude  estimation  was  performed  to  update  the  Z  coordinate  of  a 
foothol d. 

The  autnor  would  like  to  emphasize  again  that  the  subjects  of  atti¬ 
tude  control  and  active  compliance  have  been  investigated  previously  in 
detail  by  Pugh,  and  the  results  were  well  documented  in  [46].  Since 
they  were  incorporated  into  the  author’ s  software  to  help  resolving  the 
problem  of  rough  terrain  motion,  brief  descriptions  were  included  in 
this  chapter  for  the  completeness  of  this  dissertation. 
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Chapter  7 


EVALUATION  OF  THE  VISION  SYSTEM  AND  EXPERIMENTAL  RESULTS 

7.1  Introduction 

The  goal  of  FTL  operation  is  to  enable  the  Hexapod  to  walk  over 
even/rough  terrain  under  the  guidance  of  a  human  operator.  Specifi¬ 
cally,  the  front  feet  should  step  onto  the  footholds  assigned  by  the 
operator,  the  middle  legs  should  follow  the  footprints  of  the  front 
legs,  and  the  rear  legs  must  follow  the  steps  of  the  middle  feet. 
Therefore,  the  overall  performance  of  FTL  operation  depends  on  three 
factors:  (1)  the  accuracy  of  the  vision  system,  (2)  the  accuracy  of  the 
servo  control  loop,  and  (3)  tne  precision  by  which  a  foothold  is  updated 
from  one  locomotion  cycle  to  another.  While  (1)  and  (2)  determine  how 
close  a  front  foot  can  step  onto  a  specified  foothold,  factors  (2)  and 
(3)  dictate  whether  a  rear  or  a  middle  leg  can  truly  follow  their 
predecessors.  In  Section  7.2,  the  vision  system  is  evaluated  in  terms 
of  its  accuracy,  cost  and  reliability.  Experimental  results  of  a 
typical  FTL  operation  are  given  and  discussed  in  Section  7.3. 

7.2  Evaluation  of  the  Vision  System 
7.2.1  Accuracy 

To  find  out  how  precise  the  vision  system  is  capable  of  locating  a 
foothold,  the  following  test  was  performed.  With  the  Hexapod  standing 
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at  a  fixed  location,  a  number  of  points  inside  its  field  of  view  were 
selected  and  their  coordinates  were  measured  manually  with  respect  to 
the  botfy  coordinate  system.  The  laser  beam  was  then  pointed  at  the 
selected  spots,  one  at  a  time,  the  vision  system  was  activated  to  sense 
the  image,  and  the  coordinates  were  computed  by  triangul ation.  This 
arranganent  is  shown  in  Fig.  7.1.  The  results  are  tabulated  in  Table 
7.1,  where  RX,  RY,  and  RZ  represent  the  coordinates  measured  by  hand; 

I X 1 ,  IY1,  1X2,  IY2  are  the  image  coordinates  decoded  by  the  interface 
circuit;  and  CX,  CY,  CZ  are  the  coordinates  computed  using  triangu¬ 
lation.  For  the  35  entries  of  data  shown  in  the  table,  the  average 
errors  of  the  X,  Y,  and  Z  coordinates  are  1.34  cm,  1.16  cm,  and  1.2  cm, 
respecti vely . 

The  locating  of  a  testing  point  may  also  be  considered  as  the 
determination  of  a  vector  from  the  middle  of  the  two  cameras  to  the 
testing  point.  The  lengtn  of  this  vector,  *R,  is 

1R  =  / ( RX  ) ^  +  (RY)2  +  (RZ+50)*  (7.1) 

An  offset  of  50  cm  is  added  to  RZ  because  the  cameras  are  50  cm  higher 
than  the  reference  point  with  respect  to  which  measurements  are  per¬ 
formed.  The  error  of  a  measurenent,  m,  is 

A*  =  / (RX-CX  )2  +  (RY-CY)2  +  (RZ-CZ  )2  (7.2) 
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TABLE  7.1 

Data  of  Triangulation  Experiments. 


(RX 

,  RY, 

and 

RZ  are 

the 

actual 

coordinates  of  a 

testing  point 

;  1X1, 

IY1, 

1X2, 

and 

IY2  are 

the  image 

coordinates; 

CX,  CY, 

CZ 

are  the  computed  values 

by 

triangul  ation. ) 

RX 

RY 

RZ 

1X1 

I Y 1 

1X2 

IY2 

CX 

CY 

CZ 

(cm) 

(cm) 

(cm) 

(cm) 

(cm) 

(cm) 

1 

24 

90 

77 

193 

151 

207 

123 

25.65 

87.7 

77.43 

2 

24 

67.5 

77 

177 

150 

190 

126 

25.15 

64.7 

76.1 

3 

24 

44 

77 

154 

150 

170 

130 

24.5 

43.27 

78.09 

4 

24 

21 

77 

128 

147 

147 

134 

24.17 

19.93 

77.92 

5 

24 

0 

77 

106 

144 

125 

138 

24.09 

-0.2 

79.7 

b 

24 

-24.5 

77 

78 

142 

97 

142 

23.09 

-25.85 

73.99 

7 

24 

-47.5 

77 

56 

139 

72 

145 

22.54 

-48. 

78 

8 

24 

-70.5 

77 

36 

135 

48 

146 

23.1 

-72.4 

79.56 

9 

24 

-93 

77 

19 

132 

27 

148 

22.54 

-95.6 

79.57 

10 

46 

90 

77 

197 

123 

201 

96 

49.5 

89.03 

73.67 

11 

46 

67.5 

77 

176 

120 

184 

99 

48.15 

64.67 

76.67 

12 

46 

44 

77 

154 

118 

165 

101 

47.73 

42.8 

76.53 

13 

46 

21 

77 

130 

116 

143 

103 

47.43 

20.13 

76.57 

14 

46 

0 

77 

107 

114 

121 

107 

45.79 

-1.46 

74.6b 

15 

46 

-24.5 

77 

82 

111 

95 

111 

46.5 

-26.13 

77.78 

16 

46 

-47.5 

77 

61 

110 

72 

114 

45.4 

-47.6 

77.01 

17 

46 

-70.5 

77 

42 

108 

50 

116 

45.25 

-69.6 

76.33 

13 

46 

-93 

77 

25 

106 

29 

119 

45.52 

-93.8 

77.7 
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TABLE  7.1  -  continued 


RX 

RY 

RZ 

1X1 

I Y 1 

1X2 

IY2 

CX 

CY 

CZ 

(cm) 

(cm) 

(cm) 

(cm) 

(cm)  i 

(cin) 

19 

45.5 

85 

74 

195 

121 

200 

96 

48.7 

84.6 

74.87 

20 

45.5 

62.5 

74 

174 

119 

133 

98 

47.18 

60.9 

72.73 

21 

45.5 

40 

74 

150 

117 

163 

101 

47.10 

38.62 

74.38 

22 

45.5 

17 

74 

125 

115 

139 

104 

46.18 

15.37 

73.44 

23 

45.5 

0 

74 

107 

113 

121 

106 

46.27 

-1.46 

73.88 

24 

45.5 

-29 

74 

77 

110 

91 

112 

44.81 

-29.49 

73.5 

25 

45.5 

-51.5 

74 

55 

no 

66 

115 

43.4 

-51.94 

72.78 

26 

45.5 

-74.5 

74 

37 

108 

45 

119 

43.01 

-73.68 

73.6 

27 

45.5 

-97 

74 

20 

107 

24 

122 

42.5 

-97.53 

74.28 

28 

23 

85 

74 

197 

150 

207 

122 

25.19 

83.45 

71.9 

29 

23 

62.5 

74 

175 

151 

189 

126 

24.07 

61.4 

72.73 

30 

23 

40 

74 

150 

149 

167 

131 

24.22 

39.45 

76.75 

31 

23 

17 

74 

124 

147 

144 

135 

23.16 

16.31 

74.97 

32 

23 

0 

74 

104 

145 

125 

139 

21.98 

-1.09 

73.13 

33 

23 

-29 

74 

73 

142 

93 

144 

21.68 

-29.18 

75.59 

34 

23  - 

51.5 

74 

51 

139 

67 

147 

21.05 

-51.47 

74.54 

35 

23  - 

74.5 

74 

31 

136 

44 

148 

20.2 

-72.89 

71.83 
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Tne  average  error,  A£,  of  the  35  tests  is 


35 

E  lAAil 

AE  =  ~T5 -  =  2*3688  (Cm)  (7,3) 

while  the  average  vector  length,  AVL,  is 


35 

z  JiR-j 

AVL  =  -  =  140  (cm)  (7.4) 

Ob 


and  the  percentage  error,  E,  is 


E 


AE 

AVL 


100% 


1.69% 


7.2.2  Cost 


(7.5) 


The  components  comprising  the  vision  ystem  and  their  individual 
prices  are  listed  in  Table  7.2. 
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TABLE  7.2 

Components  and  Costs  of  the  Entire  Vision  System 


Item  Quantity 

Cost 

Camera  and  CCU 

2 

$7000.00 

CRT  monitor 

1 

670.00 

45  ft.  cable 

2 

450.00 

Fi sheye  lens 

2 

330.00 

Optical  filter 

2 

302.00 

Laser  head 

1 

400.00 

Laser  power  supply 

1 

250.00 

Bal 1  socket  head 

2 

40.00 

Interface  board 

1 

300.00 

IC  chips 

86 

100.00 

Power  supply  unit 

1 

100.00 

TOTAL 

$9942.00 

The  total  cost  of  the  entire  vision  system  is  nearly  $10,000.,  with 
the  cameras  and  their  CCU's  contributing  to  two  thirds  of  the  total 
amount.  The  optical  radar  system  constructed  by  ERIM  is  estimated  to  be 
about  $200,000.,  which  is  20  times  as  expensive  as  this  vision  system 
[47].  Moreover,  except  for  the  special  purpose  interface  circuit,  most 
of  the  components  can  be  used  for  other  applications.  For  example,  the 
laser  unit  may  be  utilized  for  experiments  which  require  a  laser  beam  as 
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a  source,  and  the  CRT  monitor  can  be  connected  to  any  standard  video 
signal  generator  as  a  display  instrument.  3y  adding  more  equipment,  for 
instance,  a  simple  image  processor  which  can  store  one  frame  of  image 
data,  the  potential  of  the  TV  cameras  can  be  further  exploited  as  a 
useful  tool  for  research  work  in  the  areas  of  image  processing  and 
pattern  recognition.  Hence,  the  vision  system  not  only  is  a  crucial 
element  for  the  successful  implementation  of  FTL  operation,  but  also 
provides  possible  opportunities  for  other  research.  As  such,  its  cost 
is  considered  worthwhile  and  well-justified. 

7.2.3  Reliability 

At  the  time  of  this  writing,  the  binocular  vision  system  has  been 
installed  and  operated  for  the  past  6  months.  Not  a  single  case  of 
mal function,  either  in  hardware  or  in  software,  has  been  observed  so 
far.  It  is  thus  concluded  that  the  whole  system  is  highly  reliable. 

7.2.4  Operational  Speed 

After  a  foothold  is  selected  by  the  operator,  it  takes  three  steps 
to  obtain  the  spatial  coordinates.  First,  the  hardware  circuit  needs 
time  ti  to  decode  the  image  coordinates.  Second,  it  takes  time  t2  to 
transfer  the  5  bytes  of  data  to  the  computer.  Third,  the  triangul ation 
software  needs  time  t3  to  compute  the  coordinates.  Since  the  cameras 
are  operated  at  a  frequency  of  30  frames  per  second  and  a  complete  image 
detecting  cycle  takes  up  to  4  frames  (since  there  are  4  states  for  each 
image  decoder),  t\  is  approximately  120  msec.  Using  the  maximum  baud 
rate,  %00,  t£  is  5  msec.  The  triangul ation  subroutine  takes  0.6  msec, 
to  perform  the  computation.  Thus,  the  total  time  needed  is  125.6  msec. 
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Obviously,  the  bottle  neck  is  in  the  image  detecting  phase.  Neverthe¬ 
less,  the  vision  system  is  only  activated  twice  during  a  locomotion 
cycle  to  acquire  image  data,  and  the  current  operational  speed  is  more 
than  sufficient.  If  it  is  to  be  used  to  collect  terrain  data  and  to 
build  a  map  point  by  point,  higher  speed  is  desirable.  Improvement  is 
possible  by  employing  a  high  speed  camera,  such  as  the  one  used  in  the 
Ohio  State  University  Gait  Laboratory  [67],  and  by  parallel  data  trans¬ 
mission  between  the  interface  circuit  and  the  11/70  computer. 

7.2.5  Limitations  of  the  Vision  System 

There  are  a  few  limitations  on  the  vision  system: 

(1)  It  will  detect  only  one  image  for  the  whole  frame.  Since  it  is  de¬ 
cided  that  only  one  foothold  will  be  assigned  at  a  time,  as  far  as  FTL 
operation  is  concerned,  this  is  not  considered  a  disadvantage.  Multiple 
image  detection  would  become  possible  if  more  hardware  were  to  be  added 
to  the  existing  interface  board. 

(2)  Tne  laser  beam  must  be  projected  at  points  which  are  sufficiently 
close  to  the  cameras  so  that  the  intensity  of  the  laser  image  is  strong 
enough  to  activate  the  photo  sensors.  In  other  words,  there  is  a 
limited  range  within  which  a  laser  spot  will  be  sensed.  Furthermore, 
the  light  source  should  be  inside  the  common  field  of  view  of  the 
cameras.  It  turns  out  that  the  latter  requirement  overrides  the  former 
one,  i.e.,  the  laser  image  must  be  projected  within  the  2  m  by  2  m  field 
of  view. 
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(3)  The  laser  beam  must  be  pointed  on  an  object  which  will  reflect 
enough  light  to  be  picked  up  by  the  cameras.  For  example,  if  the  laser 
beam  is  pointed  to  a  black  object,  no  image  can  be  detected  by  the 
system  and  FTL  operation  will  not  be  completed. 

7.3  Experimental  Results 

To  monitor  if  each  leg  really  steps  onto  its  foothold,  data  of  the 
actual  foot  positions  and  the  foothold  locations  are  recorded  and  stored 
when  the  Hexapod  is  in  motion,  and  then  examined  and  analyzed  off-line. 
Fig.  7.2  through  Fig.  7.7  represent  typical  results  obtainable  from  a 
FTL  operation  for  a  complete  cycle.  Three  figures  are  related  to  each 
leg,  representing  the  X,  Y,  and  Z  coordinates,  respectively,  of  the  foot 
tip  position  and  the  foothold  location. 

Fig.  7.2  shows  the  X  component  of  the  actual  foot  tip  position  of 
leg  1,  noted  as  XPA[1],  and  the  corresponding  component  of  the  assigned 
foothold  for  it,  FTHD[l,x],  as  functions  of  time.  Both  are  expressed  in 
the  body  coordinate  system.  Initially,  XPA[1]  has  a  value  of  14  inches, 
and  tne  specified  foothold  is  32  inches  away  from  the  center  of  tne 
body.  During  period  [0,a],  leg  1  is  moving  forward  to  its  destination. 
As  can  be  seen  from  the  figure,  the  solid  line  meets  the  dashed  line  at 
point  a,  indicating  that  leg  1  indeed  reaches  the  specified  position. 
During  interval  [a,b],  leg  2  is  in  motion  and  both  leg  1  and  the  body 
remained  at  their  previous  locations,  therefore  the  value  of  XPA[1]  does 
not  change.  Since  XPA[1]  will  become  the  foothold  of  leg  3  in  the 

upcoming  cycle,  its  value  must  be  updated  whenever  the  body  is  moving. 

This  is  observed  in  the  figure  throughout  interval  [b,c],  as  the  value 

of  XPA[1]  decreases  from  32  to  23,  implying  that  the  body  has  moved 
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Figure  7.3  Y-Coordinate  of  the  Actual  Foot  Position  of  Leg  1,  YPA[1] 
versus  that  of  the  Specified  Foothold,  FTHD  [1,Y]. 


Figure  7.4  Z-Coordinate  of  the  Actual  Foot  Position  of  Leg  1,  ZPA[1  ] 
versus  that  of  the  Specified  Foothold,  FTHD[1,Z], 


Figure  7.6  Y-Coordinate  of  the  Actual  Foot  Position  of  Leg  3,  YPA[3], 
versus  that  of  its  Foothold,  FTHD[3,Y], 


forward  for  a  distance  of  9  inches.  During  [c,d],  leg  5  moves,  another 
foothold  is  assigned,  and  then  leg  2  transfers  toward  it.  The  constant 
value  of  XPA[1]  again  indicates  that  neither  the  body  nor  legl  moves  in 
this  period.  Tne  second  body  motion  occurred  between  d  and  e,  and 
XPA[1]  is  modified  to  a  new  value.  Finally,  leg  6  takes  off  in  [e,f] 
and  the  whole  cycle  is  completed.  To  sunmarize,  Fig.  7.2  conveys  the 
following  information:  first,  leg  1  steps  onto  the  assigned  foothold  as 
required,  and  second,  the  foothold  is  properly  updated  for  the  next 
locomotion  cycle. 

Fig.  7.3  concerns  the  Y  component  of  leg  1  and  its  foothold.  In 
this  case,  the  Y  coordinate  of  the  assigned  foothold  is  -20  inches  and 
that  of  the  initial  position  of  leg  1  is  -25.  When  moving  in  the  air, 
YPA[1 ]  approches  gradually  toward  the  prescribed  value  and  finally  meets 
it  as  it  is  placed  down.  Again,  the  agreement  of  the  solid  and  the 
dashed  lines  implies  that  the  motion  of  leg  1  in  the  Y  direction  com¬ 
plies  with  the  requirement. 

Fig.  7.4  illustrates  the  behavior  of  leg  1  in  the  Z  direction. 
Originally,  leg  1  is  on  the  floor  thus  ZPA[1]  has  a  initial  value  of  0 
inch.  The  specified  foothold  lies  on  a  wooden  block  which  is  about  4 
inches  high.  Hence  after  leg  1  is  placed  down,  its  foot  tip  position  is 
4  inches  above  the  floor. 

The  next  three  figures.  Fig.  7.5  through  Fig.  7.7,  document  the 
action  of  leg  3  during  the  cycle.  They  are  parallel  to  those  three 
which  have  just  been  described,  and  can  be  interpreted  in  exactly  the 
same  way.  There  is  one  important  connection  between  the  figures  of  leg 
3  and  those  of  leg  1.  Since  leg  must  move  to  the  location  which  is 
previously  occupied  by  leg  1,  the  initial  foot  position  of  leg  1  should 
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be  consistent  with  the  foothold  position  of  leg  3  in  all  X,  Y,  and  Z 

directions.  Comparing  Fig.  7.2  with  Fig.  7.5,  it  can  be  seen  that 

XPAtl]  has  an  initial  value  of  14  inches,  so  does  FTHD[3,x].  Similarly, 
Y  PA  Cl]  in  Fig.  7.3  has  an  initial  value  of  -24  inches  and  FTHD[3,y]  in 
Fig.  7.6  also  has  the  same  value,  and  ZPA[1]  in  Fig.  7.4  begins  with  a 
value  of  u  inch  which  agrees  with  the  value  of  FTHD[3,z]  in  Fig.  7.7. 

Fig.  7.8  to  Fig.  7.10  present  a  sequence  of  pictures  taken  during  a 
FTL  locomotion.  In  Fig.  7.8,  leg  1  has  just  been  placed  down  on  the 

tiny  bright  spot  which  is  the  image  of  a  laser  beam.  In  Fig.  7.9, 

another  foothold,  the  bright  image  on  the  floor,  is  specified  for  leg  2, 
which  is  moving  toward  it.  Fig.  7.10  shows  that  leg  2  indeed  steps  onto 
the  assigned  foothold. 

7.4  Summary 

The  performance  of  FTL  operation  depends  on  the  accuracy  of  the 
vision  system  in  locating  the  specified  foothold,  and  the  precision  of 
the  servo  control  loop  in  moving  a  leg  to  a  commanded  location.  Also, 
coordinates  of  footholds  should  be  updated  accurately  for  usage  in 
subsquent  locomotion  cycles. 

The  constructed  vision  system  costs  about  $10,000.  Its  accuracy, 
in  average,  is  about  1.69%  of  the  distance  vector  between  a  testing 
point  and  the  middle  of  the  two  cameras.  It  takes  around  125  msec,  to 
detect  an  image  and  to  compute  its  3-dimensional  coordinates.  Its 
operations  are  subject  to  the  following  restrictions:  (1)  only  the 
first  bright  image  is  detected  in  a  frame,  (?)  The  image  must  lie 
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Figure  7.9  Picture  Showing  Leg  2  in  the  Process 
of  Moving  Toward  an  Assigned  Foothold. 


Figure  7J0  Picture  Showing  Leg  2  Stepping  onto  an 
Assigned  Foothold. 
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inside  the  common  field  of  view  of  the  cameras,  and  (3)  the  laser  beam 
must  be  aimed  on  the  surface  of  an  object  which  allows  sufficient  re- 
fl  ection. 

Typical  results  obtainable  from  FTL  operation  were  presented  as 
sets  of  figures  which  depict  the  location  of  the  assigned  footholds  and 
trajectories  of  legs.  The  results  indicate  that  legs  indeed  steps  onto 
the  footholds,  thus  satisfying  the  requirement  of  FTL  operation. 
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Chapter  8 

SUMMARY  AND  CONCLUSION 

8.1  Research  Contribution 

As  an  intermediate  step  toward  the  realization  of  a  fully 
autonomous  walking  vehicle,  in  this  research  Follow-The-Leader  opera¬ 
tion  was  studied  and  successfully  implemented.  This  achievement  was 
made  possible  by  first  conceptualizing,  then  defining  the  problems, 
and  finally  pursuing  appropriate  solutions.  Hardware  and  software 
needed  to  facilitate  these  solutions  were  then  designed,  constructed,, 
tested,  and  integrated.  As  a  result,  less  decision-making  effort  is 
needed  from  an  operator  to  assist  the  Hexapod  walking  over  even/rough 
terrain  than  that  in  a  joystick  control  mode,  which  implies  a  signifi¬ 
cant  increase  in  the  capability  of  the  Hexapod.  All  in  all,  at  the 
completion  of  this  dissertation,  the  Hexapod  was  able  to  walk,  step- 
by-step,  over  rough  terrain  under  the  guidance  of  an  operator. 

With  regard  to  hardware,  a  binocular  vision  system  based  on  two 
solid-state  TV  cameras  was  built.  The  components,  such  as  cameras, 
lenses,  laser  unit,  etc.,  were  selected  after  careful  study  and  comparisons 
so  that  the  requirements  regarding  field  of  view,  signal -to-noise 
ratio,  installation  convenience,  etc.,  were  all  satisfied.  A  special- 
purpose  interface  circuit  which  served  as  the  image  coordinate  decoder 
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as  well  as  the  communication  channel  between  cameras  and  a  PDP-11 /70 
computer  was  designed  and  implemented.  Cameras  were  calibrated  to 
provide  an  accurate  camera  model.  The  fisheye  lenses  were  included 
in  this  calibration  to  compensate  distortion.  The  geometric  relation¬ 
ship  between  cameras  and  the  Hexapod  were  identified.  A  triangulation 
program  was  developed  to  reconstruct  the  three-dimensional  coordinates 
of  an  observed  light  source.  Consequently,  a  reliable  vision  system 
which  is  able  to  recognize  specified  footholds  and  locate  their 
positions  is  now  available  to  the  Hexapod. 

Before  this  work.  Hexapod  locomotion  was  limited  to  wave  gaits 
based  on  considerations  of  stability  margin,  and  the  movements  of  all 
legs  had  to  accommodate  the  motion  of  the  center  of  the  body.  For 
FTL  locomotion,  however,  the  center  of  the  body  has  to  accommodate  the 
motion  of  legs  and  wave  gaits  are  not  applicable  to  this  case.  Thus, 
a  new  method  of  coordinating  stepping  of  successive  legs  so  that  only 
footholds  which  have  been  tested  by  preceding  legs  are  used  was  dis¬ 
covered.  Particularly,  an  algorithm  was  conceived  to  direct  the 
center  of  the  body  toward  *  location  which  would  provide  an  optimal 
stability  margin.  While  a  general  theory  which  covers  all  classes  of 
possible  gaits  and  guarantees  an  optimal  margin  does  not  exit,  it  is 
believed  that  this  algorithm  can  be  used  in  locomotion  with  any  kind 
of  gait.  As  such,  it  makes  feasible  the  automatic  control  of  the 
Hexapod  during  climbing  of  large  obstacles. 
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8.2  Research  Extensions 

As  this  work  comes  to  an  end,  it  becomes  clear  that,  with 
additional  work,  the  Hexapod  can  be  made  more  versatile  and  its  per¬ 
formance  upgraded.  With  this  in  mind,  a  number  of  suggestions  are 
given  below. 

(1)  Terrain  modeling.  An  automatic  scanning  system,  which 
controls  the  laser  beam  and  causes  it  to  sweep  over  an  area  under  the 
command  of  a  computer,  would  enable  the  Hexapod  to  create  a  terrain 
map  based  on  which  terrain  analysis  could  be  carried  out. 

(2)  Terrain  analysis.  A  series  of  algorithms  should  be 
developed  for  the  Hexapod  toselect  good  footholds  from  a  terrain 
model.  With  this  capability,  the  Hexapod  would  be  able  to  choose  a 
proper  route  and  navigate  itself  through  rough  terrain. 

The  combination  of  (1)  and  (2)  would  result  in  an  automatic 
navigation  system  for  the  Hexapod.  The  human  operator  could  be 
eliminated  and  the  vehicle  would  become  fully  automatic. 

(3)  Image  processing  and  pattern  recognition.  Currently,  only 
one  image  was  detected  for  a  frame.  This  means  that  most  of  the  infor¬ 
mation  contained  in  a  picture  was  not  used.  To  exploit  the  merits  of 

a  vision  system  to  the  greatest  extent  possible,  an  image  processor 
could  be  added  to  the  existing  system,  and  scene  analysis  could  be 
performed  based  on  the  intensity  data.  For  example,  the  boundary  of 
two  objects  may  be  mapped  into  edges  in  a  picture.  Information  ex¬ 
tracted  from  intensity  data  and' that  obtained  from  distance  data  may 
complement  each  other  and  provide  a  certain  degree  of  redundancy. 


244 


(4)  Real-time  experiments  using  different  types  of  gaits. 
Although  the  walking  algorithms  developed  in  this  work  were  mainly 
for  FTL  operation,  the  restriction  stems  from  the  fact  that  the  next 
foothold  of  middle/rear  legs  overlaps  with  the  current  foothold  of 
front/middle  legs,  which  is  so  because  there  is  only  one  foothold 
available  for  each  leg  per  locomotion  cycle.  If  there  are  more  than 
one  foothold  for  every  leg,  then  the  gait  sequence  imposed  in  FTL 
locomotion  can  be  lifted  and  motions  with  various  gaits  are  ready  to 
be  tested. 

In  conclusion,  with  the  successful  implementation  of  FTL 
operation,  the  author  is  convinced  that  a  fully  autonomous  legged 
vehicle  is  a  reachable  goal,  provided  additional  instrumentation  and 
algorithms  are  available.  While  new  devices  would  provide  the  machine 
extra  information  about  the  external  world,  new  algorithms  should  be 
developed  to  best  utilize  the  available  information.  Taking  the 
realization  of  FTL  operation  as  an  example,  the  binocular  vision 
system  enables  the  Hexapod  to  perform  visual  data  acquisition,  and 
based  on  its  outcome,  the  coordinates  of  a  specified  foothold,  the 
OSP  concept  is  devised  to  use  this  piece  of  information  to  navigate 
the  vehicle  in  the  most  beneficial  way.  The  author  also  expects  that, 
during  the  course  of  building  such  a  vehicle,  various  problems  related 
to  the  nature  of  motion  of  a  biological  system  would  be  encountered, 
and  the  recognition  as  well  as  the  resolving  of  these  issues  would 
be  an  important  milestone  toward  the  understanding  of  the  behavior 
of  biological  systems. 
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GBLF37 . PAS 


( **A-«  > 


FILE:  GBLF37.PAS  > 


/••••••••••••••••••••••••••••••••••••••••*•••••••••••*•••••••*•••/ 

/*  */ 

/•  function:  this  file  contains  global  declarations  for  «/ 

/*  FTL  MOTION  SOFTWARE.  THE  EXECUTABLE  FILES  WHICH  */ 

/♦  SHARE  THESE  GLOBALS  ARE:  */ 

/*  */ 

/*  WALK37. PAS  VISION. PAS  F00T37.PAS  */ 

/*  INIT37.PAS  LINE37. PAS  SERV37.PAS  •/ 

/*  DLNK37.PAS  LIBR37.PAS  P0WR37. PAS  */ 

/•  TRION.PAS  CHECK. PAS  */ 

/••••*•••••••»••••••••••••••••••«••••»••••••••«••••••»••••#••*•••/ 

CONST 

PI  *  3.14199; 

FSCALE  «  -200.0;  /*  FORCE  SCALE  FACTOR  */ 

MAXSTROKE  -  12.0 ;  /*  MAXIMUM  FOOT  STROKE  */ 

MOVE  ■  TRUE!  /*  SWITCH  TO  ENABLE  SERVOING  */ 

NOMOVE  ■  FALSE;  /•  SWITCH  TO  DISABLE  SERVOING  •/ 


TYPE 

ARRAY6  *  ARRAY Cl.. SI  OF  REAL. 

ARRAY IS  3  ARRAYCO. . 173  OF  REAL. 

MODETYPE  »  <  RANDOM.  NEUTRAL.  PREWALK. 

CRUISE.  SIDESTEP.  TURN  >: 

AXES  *  <x.y.z>; 

VECTOR  «  ARRAYCAXES3  OF  REAL* 

MATRIX  *ARRAYC 1 . . 6 .AXES!  OF  REAL! 

HMATRIX«ARRAYC1 . .4,1 . .43  OF  REAL ; 

ACTIVITY  *  < LEGMOT I ON , BODYMOT I ON . FOOTHOLDFETCH ) ; 
LEGSET *  SET  OF  1..SI 

VAR 


MIDSTX.  MIDSTY. 

/* 

MIDSTANCE  COORDINATES 

*/ 

RPHASE. 

/* 

RELATIVE  LEG  PHASES 

*/ 

XFA ,  YFA, 

ZFA, 

/* 

ACTUAL  FOOT  FORCES 

*/ 

XFD .  YFD . 

ZFD. 

/* 

DESIRED  FOOT  FORCES 

*/ 

XSTATE , YSTATE , ZSTATE . 

/* 

FILTERED  FOOT  FORCES 

*/ 

XPA ,  YPA , 

ZPA, 

/* 

ACTUAL  FOOT  POSITIONS 

*/ 

XPD .  YPD, 

ZPD, 

/* 

DESIRED  FOOT  POSITIONS 

*/ 

XRD ,  YRD . 

ZRO. 

/* 

DESIRED  FOOT  RATES 

*/ 

ZPTERM, 

/* 

Z  POS.  TERM  FOR  ATT.  CONTROL 

*/ 

ZZERO, 

/* 

Z  COMPONENT  OF  ZEROFORCE 

*/ 

RSUPPORT 

/* 

SUPPORT  PHASE  INDICATORS 

*/ 

:  arrays; 
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ZEROFORCE. 

FORCE 

:  ARRAY18.' 


/*  true  force  offsets 
t*  TRUE  force 


*/ 

#/ 


FZERO 


/*  FLAGS  FOR  FORCE  ZEROING 
•  ARRAY Cl.. S3  OF  BOOLEAN. 


*/ 


ATTITUDE. 
COMPLIANCE, 
OPTIMIZATION, 
PASS1 , 

SAVE. 

SPRING 


:  boolean: 


/*  SNITCH  FOR  ATTITUDE  CONTROL 
/*  INDIRECT  COMPLIANCE  SNITCH 
/*  SNITCH  FOR  OPTIMAL  FORCE 
/*  SNITCH  TO  INITIALIZE  FILTER 
/*  SNITCH  FOR  DATA  AGUISITION 
/*  SNITCH  FOR  ACTIVE  COMPLIANCE 


*/ 

*/ 

*/ 

*/ 

•/ 

*/ 


COMMAND 


:  char; 


/*  OPERATOR  INPUT  COMMAND 


OATACLOCK . 
LASTCLOCK , 
TOTALCLOCK . 

TTICK, 

XPl.YPl.XP2.YPZ. 

ST0P.S8Q.AG 

:  integer; 


£™£L2TJJ£0UIsrTI0N  start 

/*  STORAGE  FOR  CLOCK  BUFFER 
/*  CLOCK  TICK  ACCUMULATOR 
/*  DATA  STORAGE  COUNTER 


*/ 

*/ 

*/ 

*/ 


MODE 


/•  HEXAPOD  OPERATING  MODE 

hodetype: 


*/ 


BETA, 

bphase, 

DPS  I. 

DT. 

DXB. 

DYB. 

DZB, 

FOOTLIFT , 

LIFTTIHE. 

MIDST2. 

NVELX.  NVELY, 

NDPSI, 

PERIOD. 

PHASE. 

PSIC. 

RADIUS. 

TPHASE. 

8PERI0D, 

VELX,  VELY. 

VELMAX , 

XT . YT . ZT . XE . YE , ZE , ZF3 
:  real; 


/«  LEG  DUTY  FACTOR  #/ 

/!  E*LTERED  TURN  RATE  COMMAND  */ 

/*  DELTA  TIME  (SEC)  #/ 


/*  FOOT  LIFTING  HEIGHT 


/*  MIDSTANCE  Z  COORDINATE 
/*  OPERATOR  VELOCITY  COMMANDS 
f*  OPERATOR  TURN  RATE  COMMAND 
/*  PERIOD  OF  KINEMATIC  CYCLE 
/•  KINEMATIC  CYCLE  PHASE 


*/ 

*/ 

*/ 

*/ 

*/ 


/*  RADIUS  FROM  CG.  TO  MIDSTANCE  */ 


/*  SUPPORT  PERIOD  (SEC) 

/#  FILTERED  VELOCITY  COMMANDS 
/*  MAX  FOOT  VELOCITY  COMPONENT 


*/ 

*/ 

*/ 


SUPPORT 


/#  SET  °F  LEGS  IN  SUPPORT  PHASE  */ 


:  legset: 

TRISET  ;  ARRAY Cl.. 20 3  OF  LEGSET 1 
FTHD , PTFTHD  I  MATRIX! 
TMATRIXIHHATRIX: 

action  :  activity; 

VTR12, VTR23  I  VECTOR: 
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(••A-*) 

<  FILE!  WALK37 . PAS  > 


/•••••••«••••••••••••••••••••••••••«•••••••••••••••••/ 

/•  PROGRAMMER  :  SHENJ-JEN  TSAI  •/ 
/•  DATE  :  APRIL  20.  1983  •/ 
/*  FUNCTION  :  THIS  PROGRAM  DIRECTS  HEXAPOD  TO  */ 
/*  PERFORM  FOLLOE-THE-LEADER  MOTION  */ 
/*  WITH  GAIT  SEQUENCE  1-3-5-2-4-B.  •/ 
/*  */ 
/*  PROCEDURES  CALLED  :  F0QT37,  INIT37,  LIBR37.  ♦/ 
/*  SERV37,  DLNK37.  LINE37.  */ 
/•  P0WR37 >  VISION.  TRION.  */ 
/*  R0QT4 .  CHECK.  •/ 
/*  */ 
/*  < NOTE  I  INIT37 .  LIBR37.  SERV37,  DLNK37 .  LINE37.  */ 
/*  AND  P0ER37  ARE  EXACTLT  THE  SAME  AS  THOSE  */ 
/•  USED  IN  VERSION  3.0  SOFTWARE.  THEY  ARE  •/ 
/•  DOCUMENTED  IN  PUGH'S  THESIS.  )  */ 
/*  */ 
/•  USER  GUIDE  I  >PAS  WALK37*GBLF37. WALK37  •/ 
/•  >HAC  WALK 37* WALK 37  •/ 
/*  >TKB  §W ALK37.CMD  */ 
/•  >RUN  MALK37  •/ 
/*  */ 
/••••••••••••••••«•••••••••••••••••••••«•••••*•••••••/ 


VAR 

component: axes; 

NEXT . CG . CGMOVE . 0LDFT1 . 0LDFT2 . NEHFT 1 . 

newft2.Pi .P2.ltgc.ptgc.ntgc  :vector; 

XTEMP , YTEMP . ZTEMP . ERR . XG  >  YG  >  ZG . SAI . TH1 . TH2 . 
XST9IDE . YSTRIDE » XABSTK . YABSTK . 

TT I ME . DVELX . DVELY . DDPS I , P I TCH . ROLL , 

ANGCG , AUXANG . FGA I N . VMAX . STROKE . PT IME . LTI ME . 
LANG . RANG . ANG . MAXANG . A . B . C . TUR ANG . RESANG . 
DXCG . D YCG . DANGCG . T ANGCG : REAL i 
J . LEGL I FT , I D . I . NT: INTEGER I 
switch: char; 

SAFE . CONTACT : BOOLEAN ; 

HI PX . OLDXPA . OLDYPA . OLDZPA I ARRAYS ; 

topblock: array is; 

LABEL  20.30; 

FUNCTION  ATAN2( Y.  X  \  REAL)  :  REAL;  EXTERNAL I 
FUNCTION  ASIN( VALUE  i  REAL)  1  REAL.'  EXTERNAL; 

function  sign<x:real>:real;  external; 


PROCEDURE  CLOCK  INI T;  EXTERNAL! 
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PROCEDURE 

PROCEDURE 

PROCEDURE 

PROCEDURE 


vision;  external; 
normalize;  external: 

R8 ADC  <  NOCHAN .  PRSTCH :  INTEGER  SCALE  I  REAL 1 
VAR  INDATA.'ARRAYIS)  ;  EXTERNAL ; 

CHECK (XG.YG.ZG  IREAL)  VAR  SAI > TH1 > TH2  I REAL) ’ 

external: 


PROCEDURE 

PROCEDURE 

PROCEDURE 

PROCEDURE 

PROCEDURE 

PROCEDURE 

PROCEDURE 

PROCEDURE 


initialize;  external; 

halt;  external: 

footpath;  external; 

testqn;  external; 

turnon;  '  external; 

turnof;  external; 

jservo<move:  boolean  >:  external; 

turnangle;  <  compute  the  turning  angle  > 


BEGIN 

vtri2cxi:«ptgccx3-ltgccxj; 

VTR12C  Y 3 : «PTGCC YI-LTGCC Y J  * 

VTR23IX3 I -NTGCCX3-PTGCCX3 ) 

VTR23C Y3 I *NTGCC Y3-PTGCI Y3 ; 

A; *SQRT < VTR12C  Xl*VTRi2CX3*VTRiZtY3*VTRl2£Y3  > ’ 
B:*SQRT<VTR23CX3*UTB23CX3«-VTR23tY3*VTP23CY3>  ; 
C:=<VTR12CX3*VTR23£X3«-VTR12tY3*VTR23tY3  >/ (A*B) I 
IF  ABS(C)  >  0.9S9S  THEN  ANGI-O. 

ELSE  BEGIN 

IF  VTR23CY3  >  VTR12CY3  THEN  A.*  »SGRT(  1  .-C*C> 

else  a:«-sqrt<i.-c*c>: 
ang:»atan2<A'C>: 

IF  ABS(ANG)  >•  (3. 14138/2. ) 

THEN  BEGIN 

ang:>ang-3.i4Iss; 

IF  ABS(ANG)  >-  3.14159  THEN  ANG: «ANG+2.*3. 14159 

else; 

END 

else; 

end; 

ang:>ang+resang; 

IF  ABS(ANG)  >  ABS(MAXANG)  THEN  TURANGI »MAXANG*ANG/ABS( ANG ) 

else  turang:«ang; 

NDPS I : - TUR ANG / PER I OD ; 

ANG : *  TUR AND* 1 80 . /3 . 141591 
END;  <  OF  TURNANGLE  > 


PROCEDURE  SYNTHMOTION)  <  COMPUTE  X  ANO  Y  VELOCITIES  > 


BEGIN 

dt:»o. : 

xstrioe:*i.z*<ntgccx3-ptgccxi>: 
ystride:«i .2*<ntgc:y3-ptgccy3> : 
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xabstk:«abs<xstride> :  yabstk : *abs<  ystride; ; 

IF  XABSTK  >«  YABSTK  THEN 
BEGIN 

vmax: ■xabstk/period; 

IF  XABSTK  >■  O.OOl  THEN 
BEGIN 

nvelx:»vmax*xstride/xabstk: 
nvely: «vhax*ystride/xabstk : 

END 

ELSE 

BEGIN 

nvelxi-o.;  nvely: *0. * 
end; 

END 

ELSE 

BEGIN 

vhax: -yabstk/period; 

IF  YABSTK  >«  0.001  THEN 
BEGIN 

nvelx : »VMAX*XSTR IDE /YABSTK ; 
nvely: »vmax#ystride/yabstk; 

END 

ELSE 

BEGIN 

nvelx:«o.;  nvely: «o. ; 
end; 

end; 

eno:  <  OF  synthhotign  > 

procedure  newdata;  <  fetch  the  next  foothold  > 

VAR  xd.yd.zdireal; 

BEGIN 

vision; 

next: X  3 : «XE* 1 00 . /2 . 34*22 . 75* 1 . 5 ; 

NEXTtYi:«YE*100./2.34; 

NEXTCZ3:*ZE*100./2. 34-10. 31 

IF  NEXTCY1  >•  0.  THEN  <  FOOTHOLD  IS  FOR  LEG  2  > 

BEGIN 

FTHDC2.Xi:-NEXTrX3-0.23; 

FTHDC2. Y3 I “NEXT! Y 3 *0.4; 

FTHDI2  >23 : "NEXTCZ3  I 

id:*2;  leglift:«2: 

NTGCCX3 I « (XPAC1 3*XPAC33*XPAC53*FTHDCZ»X3 
♦FTHDC4,X3*FTHD£6rX3>/B.: 

NTGCC  Y3  .'  »  ( YPAC  1  3*YPAC33*YPAC53*FTHDC2. Y3 
♦FTHDC4. Y3*FTHDCB» Y3 )/S. t 

END 

ELSE  <  FOOTHOLD  IS  FOR  LEG  1  > 

BEGIN 

FTHDC 1 . X3 I “NEXT CX3 ; 

FTHDC l »Y3:*NEXTCY3' 

fthdci.z::»nextiz3; 
id: - i :  leglift:-i; 

NTGCC X 3  I ■ ( XPAC23*XPAC43*XPAtB3*FTHDt 1 » X3 
*FTHDC3»  X3*FTHDC3. X3  > /S. ; 

NTGCC Y 3  I » ( YPAC 23*YPAt 4  3+YPACB3+FTHDC 1 » Y3 
*FTHDC3» Y3*FTHDC5»Y3  >/B. 1 

end; 
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action: -legmotion: 
support: «support-cleglift3; 
end; 

PROCEDURE  SAFECKI  <  SIMULATE  THE  MOTION  AND  SEE  IF  ANV  LEG 
HILL  BE  OUT  OF  THE  KINEMATIC  MARGINS  > 


CONST  YHIP-8.562; 

uar  ick: integer; 

dxsm.dysm,b.c:real: 

fthdsmimatrix; 

BEGIN 

safe:-true;  sai:»o.;  thi:«o.;  th2:*o.: 

<  CHECK  IF  LEG  3<4)  UILL  BE  OUT  OF  MARGIN  > 

ick:«leglift+2; 

xgi-fthdcick.x^-hipxcicki; 

YG  .*  » ABS  <  FTHD C  I CK  .  Y 3  > - YH I P I 

zg:-fthdcick.z3: 

CHECK  < XG ,  YG .  ZG .  SA I ,  TH1 ,  TH2  >  • 

SAI I *ABS ( SAI ) *  THi:»ABS(THI) :  TH2 : * ABS <  TH2 ) * 

IF  (SAI  >  75 . )  OR  < TH1  >  75.)  OR  ( TH2  >  75.) 

THEN 

BEGIN 

WRITELN<  '  LEG  '.ICK II.'  IS  OUT  OF  MARGIN  *1  ■  ')  I 

UR  I TELN  (  '  S  A I  *  ' .  S  A I :  B  1 4  .  '  THl  -  ' .  THI  IBM,  '  TH2- ' » 
th2:s:4>; 

safe: -false; 

END 

else: 

<  CHECK  IF  ANY  LEG  UOULO  BE  OUT  OF  MARGIN  IF  THE  BODY  MOVES 
TO  ITS  DESTINATION  > 

b:-sin<turang>;  c: »cos<turang) : 
dxsm:-xstride/i.2:  dysm:»ystride/i.2; 
for  i:»i  to  6  DO 

BEGIN 

IF  ((ID-1)  AND  <(1-1)  OR  ( 1-3) > )  OR  ((ID-2) 

AND  ( ( 1-2)  OR  (1-4)  )> 

THEN 

BEGIN 

FTHDSMC I . X  3 : • ( FTHD 1 1 , X  3 -DXSM ) *C+ ( FTHD  C I . Y  3 -D YSH  >  *B i 

FTHDSMC I , Y3 : — ( FTHDC I , X3-DX3M  >  *B+ ( FTHDC I , Y3-DYSM)*C 

XG:  -FTHDSMC  I .  X 3-HI PXC  I  3 

YG I -ABS ( FTHDSM C I . Y 3  > - YH I PI 

ZG: -FTHDC I.Z3I 

END 

ELSE 

BEGIN 

XG I - ( PTFTHD C I , X3-DXSM) *C* <  PTFTHDC I . Y  3-DYSH ) *B 
-HIPXCI3; 

YG : - - <  PTFTHD  C I . X  3 -DXSM  >  *B* ( PTFTHDC I , Y  3 -DYSM ) *C  5 
YG:-ABS( YGJ-YHIP! 

zg:-ptfthdci.Z3: 

end; 

CHECK  ( XG ,  YG .  ZG .  SA  I .  TH 1 .  THZ ) ; 

SAI : -ABS (SAI);  THt:-ABS(THl>;  TH2 : -ABS ( TH2  > “ 
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IF  ( SAI  >  73.)  OR  < THI  >  73.)  OR  ( TH2  >  73.) 

THEN 

BEGIN 

WRITELN<  '  LEO  '.III.'  IS  OUT  OF  HARGIN  #2  •  ' ) I 
URITELN(  'SAI*'.SAi:S:4.  '  THI « ' . TH1 1 8 14 ,  '  TH2- ' . 

th2:b:4>; 

safe: -false; 

END 

else; 

end;  <  of  do  > 

<  CHECK  IF  LEG  5(6)  HILL  BE  OUT  OF  HARGIN  AFTER  THE  BODV 
MOVES  AND  AFTER  THE  LEG  TRANSFERS  > 

ick:«leglift*4; 

fthdsmi ick .x3 : * (  fthdi ick  »  xi-dxsm) *c+ (Fthdi ick . yi-dysm)*b; 
FTHDSMIICK.Y3:— <FTHDtICK.X3-DXSM>*B-MFTHDtICK,Y3-DYSN)*C; 
xg: -fthdsmi ick, xi-hi pxc ick 3; 

YO : - ABS < FTHDSM t ICK . Y  3 ) -YHIP ; 

zg:«fthdcick,zi; 

CHECK (XG . YG. ZG . SAI .  THI >  TH2) ; 

SAi:-ABS(SAI>:  THi:-ABS(THl>;  TH2:*ABS(TH2) ; 

IF  (SAI  >  75.)  OR  (THI  >  73.)  OR  (TH2  >  73.) 

THEN 

BEGIN 

MRITELNI  '  LEG  '.ICKM,'  IS  OUT  OF  MARGIN  *3  !  'X 
HR I TELN (  '  S A I •  '  >  S A 1 1 8 1 4 .  '  THI «'. THI 1814 .  '  TH2- ' . 
thz:s:4); 

safe: -false; 

END 

else; 

end;  <  OF  SAFECK  > 

PROCEDURE  FOOTHEIGHT;  <  COMPUTE  FOOTLIFT  OF  A  TRANSFER  LEG  } 

var  b.c:real: 

BEGIN 

IF  PTFTHDCLEOLIFT.Z3  >  FTHDILEDLIFT .23 
THEN  BI-FTHDtLEGLIFT.ZJ 
ELSE  8:-PTFTHDtLEGLIFT.Z3: 
c»,b-s  ; 

IF  C  >*io.  THEN  FOOTLIFT :-B-10. 

ELSE  FOOTLIFT! -3. 81 
end;  <  OF  FOOTHEIGHT  > 


BEGIN  <  BEGIN  OF  THE  MAIN  PROGRAM  > 
CLOCK  INI TJ  <  START  PROGRAMMABLE  CLOCK  > 


MIDSTXt 1 J : -22.73;  HIDSTXC23 : -22.73; 

MIDSTXC33 : -0. ;  MIDSTXt  4] : *0. • 

MIDSTXt  S3:— 22.75;  MIDSTXt  S3  .' —22. 73; 

MIDSTYtl3:— 24.;  MIDSTY 1 23  I -24. i 

MIDSTYt33:— 24. ;  HIDSTYt43 : -24. ; 

MIDSTYt33.'— 24. :  MIDSTYtS3  : -24. ; 

midstz:-2o.;  beta:»s/b; 

HIPXtl3:«22.73;  HI PXt23 : -22. 73; 

hipxi33:-o.;  HiPxt4i:-o.; 

HIPXt33:»- 22.73;  HIPXtSi:— 22.73; 
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fgain:*o.s; 
ZF5 : -MIDSTZ 


footlift:*io. : 


<  INITIALIZE  •  ,M-  COMPLIANT  TRIPOD  SETS  IN  ORDER 
OF  PREFERENCE  > 

trisetcii  :■  ci, 4. si;  trisetc23  :*  C2.3.61: 
teston; 

TURNOF ; 

WRITELNC '  TURN  ACTIVE  COMPLIANCE  ON  ?  Y/N  'll 
RE ADLN (SNITCH) 7 

IF  SNITCH  *  'Y'  THEN  COMPLIANCE I «  TRUE 
ELSE  compliance:-  false; 

WRITELNC'  TURN  ATTITUDE  CONTROL  ON  ?  Y/N  ')! 

RE ADLN (SWITCH), 

IF  SWITCH  -  'Y*  THEN  ATTITUDE! -TRUE 
ELSE  attitude: -false: 

WRITELNC'  TURN  FORCE  OPTIMIZATION  ON  ?  Y/N  ')! 

READLNC SWITCH) ; 

IF  SWITCH-  'Y'  THEN  OPTIMIZATION: -TRUE 

else  optimization: -false; 

RBADC £  5 . SB , 1 , TOPBLOCK ) i 
PITCH : -TOPBLOCK  1 73  *  C -0 . 7854  > ; 

ROLL! -TOPBLOCK C83*(-0. 7854) ! 

WRITELNC'  PITCH- ' ,PITCH:b:4, '  roll* ' ,roll:b:4) ; 
WRITELNC'  INPUT  THE  VALUE  OF  MAX.  TURANC,  IN  DEGREE  ') 
REAOLN ( MAXANG ) • 

MAXANG:-MAXANG*3. 14159/180. ; 

pass i: -false; 

normalize; 

initialize: 

<  MOVE  HEXAPOD  INTO  THE  INITIAL  POSTURE  FOR  FTL  MOTION  > 

XPDCli:«MIDSTXC13-9.; 

XPDC23 : -MIDSTXC23 I 
XPDC3K-MI0STXC33-4.5: 

XPDC4] : -MIDSTXC43+4.5; 

XPDC5K-MIDSTXC5J: 

XPDC61 : -MIDSTXC63+9. : 


FOR  I!-  1  TO  6  DO 
BEGIN 

YPDt I  3 : -MIDSTYt I  3 ;  ZPDC I  3 : -MIDSTZ; 
PTFTHDC I,X3!-XPDCI3; 

PTFTHDC I » Y3 I -YPDC 13: 

PTFTHDC I, Z3:»ZPDCI3: 

end; 

err:-iooo. : 
turnon; 

WHILE  ERR  >  1.  DO 
BEGIN 

err:«o. ; 

JSERVOCMOVE)  : 

FOR  i:-l  TO  6  DO 
BEGIN 

err:-err*abscxpacii-xpdci3) : 
end; 

end: 
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halt; 

support: -c 1,2. a, 4. 5. si; 


<  ASSIGN  FOOTHOLDS  FOR  MIDDLE  AND  REAR  LEGS  > 

FOR  I  >3  TO  6  DO 
BEGIN 

FTHDII.Xi:»XPACI-2If 
FTHDC I ,Yi:«YPACI-2J; 

FTHDC IrZi:*ZPACI-2i; 

end; 

ptgccxji-o.:  ptgccyi:-o.;  ptgcczi:«hidstz; 
ltgccx]:— 10.:  ltgccyj:-o.; 
velx:»o.;  uely:«o.:  dpsi:-o.; 
period:-u.: 

<  FETCH  THE  NEXT  FOOTHOLD  FOR  A  FRONT  FOOT. 
DETERMINE  THE  MOTION  OF  THE  BODY,  CHECK 
FOR  KINEMATIC  MARGINS  > 

safe: -false: 

WHILE  NOT (SAFE)  DO 
BEGIN 

neudata; 

footheight; 

turnangle: 

synthmotion: 

safeck: 

end; 

ttime:«o.: 

cgmovecxk-o.  :  cgmovecyi:«o.  ;  cgmoveczk-o. 
ltime:-time: 
resang: «o. ; 
lifttime:-s.: 
tphase:»o.;  bphase:-o.: 
for  component:-  x  to  z  do 
begin 

pi C COMPONENT] : -FTHDC5 , COMPONENT] ; 

P2t COMPONENT] : -FTHDC S . COMPONENT] : 

end; 

oldfti:-pi: 

0LDFT2:»P2; 

20:  cgcxi:-o.;  cgcy]:»o.;  cgcz]:-o.; 

angcg:-o. ; 
turnon; 

spring: -compliance; 

<  LOOP  FOR  A  CYCLE  OF  FTL  MOTION  » 

HHILE  NOT ( ACTION-FOOTHOLDFETCH )  DO 
BEGIN 

CASE  ACTION  OF 

bodymotion:  begin 

IF  BPHASE  <•  I.  THEN  <  KEEP  ON  MOVING  > 
BEGIN 

bphase: -bphase*dt/period: 

DUELX: -  FGAIN*(NVELX  -  VELX>: 

dvely:-  fgain*(nvely  -  vely>: 

DDPSi:-  FGAIN* ( NDPSI  -  DPSI>: 
velx:-  dvelx*dt+velx: 
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n 


vely:-  dvely*ot*vely; 
dpsi:»  ddpsi*dt*dpsi; 

END 

ELSE  <  BODY  STOP  MOVING  > 

BEGIN 

nvelx:«o.;  nvely:-o.;  ndpsi:-o.: 
velx:-o.:  vely:»o.;  dpsi:-o.: 

dt:«o. : 

for  i:>  1  TO  B  DO 
BEGIN 

PTFTHDt I >  Z1 1 -ZPAC 1 1 1 

end; 

bphase:«o.; 
act ion : -legmotion; 

LEGLIFT:»LEGLIFT*2; 

FOOTHEIGHTJ 

support:«support-cleglifti; 

end; 

end;  {  OF  BODYHOTION  > 

legmotion:  begin 

tphase: -tphase+dt/lifttime; 

IF  (TPHASE  >*  1.)  AND  (  CONTACT  )  THEN 
BEGIN 

tphase:«o.;  contact : -false; 

PTFTHDCLEGL IFT  .XI :  -XPACLEGLIFTV. 
PTFTHDCLEGLIFT , Y3 : - YPACLEGLIFT I ; 
PTFTHDCLEGL I FT  »Z3 : -ZPACLEOLIFT3  * 
SUPPORT: »SUPPORT*CLEGLIFTi; 

IF  <LEGLIFT«3»  OR  <  LEGLIFT-*> 
THEN 

BEGIN 

action: -bodymotion; 


END 

ELSE 

IF  <LEGLIFT-l>  OR  (LEGLIFT-Z) 
THEN 
BEGIN 

leglift:-leglift+z; 

footheight; 

support :«support-cleglifti 
END 
ELSE 

BEGIN 

action:- footholdfetch ; 
halt; 

spring: -compliance; 
goto  go ; 
end; 

END 

else; 

end;  <  OF  LEGMOTION  > 
end;  <  OF  CASE  ACTION  > 

JSERVO ( NOMOVE ) ; 

FOR  i:-l  TO  6  DO 
BEGIN 

OLDXPAC I I : -XPAC 131 


/ 
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oldypaci3:-ypaci3: 
oujzPAcn  :«zpAcn; 


end; 

footpath; 

JSERUO ( NOMOVE ) ; 

nt:-o:  dzb:«o.;  zfs:-o.; 

FOR  I : - 1  TO  6  DO 
BEGIN 

IF  <  I  IN  SUPPORT  )  THEN 
BEGIN 

nt:-nt+i: 

DZB : « DZB+OLDZPAC I 3-ZPAC 1 3 ; 
ZF5>ZF5+ZPAtn; 

END  else; 

end; 


<  CHECK  IF  THE  LEG  HAKES  CONTACT  WITH  GROUND  > 

IF  < TPHASE  >  0.5)  AND  < ZFACLEGLIFT3  >  15.) 
THEN  CONTACT: -TRUE 

else  contact: -false; 

dzb:-dzb/nt; 

zfs:-zf5/nt; 


<  UPDATE  THE  FOOTHOLDS  IN  BODY  CS.  AS  HEXAPOD  IB 
MOVING.  > 


b:«sin(psio ;  c:«cos<psio : 

FOR  i:-l  TO  6  DO 
BEGIN 

XTEHP : -FTHDC I ,  X3  ;  YTEHP : -FTHDC I .  Y3 ; 

ZTEMP : -FTHDC I  >Z3  ; 

FTHDC I . X I : - ( XTEMP-DXB ) *C+ ( YTEHP-DYB  >  *B ; 
FTHDC I ,Y3 :«-< XTEHP-DXB >*B+< YTEHP-DYB >*C: 
FTHDC I. Zi: -ZTEMP; 

XTEHP : - PTFTHDt I . X3 :  YTEHP : -PTFTHDI I . Y  3 : 
PTFTHDC I . X3 : - ( XTEHP-DXB ) *C+ ( YTEHP-DYB ) *B ; 
PTFTHD C I . Y3 :  —  <  XTEHP-DXB  >  *B+  <  YTEHP-DYB ) *C 
PTFTHDC I .Z3 : -PTFTHDC I .Z3 > 

end; 

XTEHP :-PTGCCX3;  YTEHP: -PTGCCY3) 

PTGCCX3 : - < XTEHP-DXB  >  *C*( YTEHP-DYB ) *B ; 

PTGC C  Y I :  —  < XTEMP-DXB  >  *B* ( YTEHP-DYB ) *C ; 

xtehp: -NTGCixa;  ytehp: -ntgccyi; 

NTGCCX3 : ■ ( XTEHP-DXB ) *C+ ( YTEHP-DYB  >  *B ; 

NTGC C  Y  3 :  —  { XTEHP-DXB ) #B+  C YTEHP-DYB ) *C ; 


ptihe  :■  tihe; 

DT  :-  < PTIHE-LTIHE)*3B00; 

ltihe:-  ptihe: 
ttihe:-ttime+dt: 

end;  {  OF  LOOP  FOR  FTL  MOTION  > 

<  COMPUTE  THE  ACTUAL  BODY  MOVEMENT  AND  TURNING  ANGLE 
BY  USING  INFORMATION  OF  TMO  SUPPROTING  LEGS  > 

NEHFT1  CX3  :  -FTHDC 3  >  X3  I  NEHFTt  C  Y3  .' -FTHDC3.  Y3  ; 
NEWFT1 C  Z  3 : -FTHDC  3 . Z I ;  NEHFTZ  C  X  3 : -FTHDC  *  »X3 ! 
NEHFT2C Y 3 : -FTHD C  4 .  Y  3  ;  NEHFTZ C Z 3 : -FTHDC 4 , Z 3  I 
B: -0LDFT1 CX3-OLDFT2CX3 ; 
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c:*oldfticv:-oldft2Iyj: 

A : * ( NEUFT 1 C  X  3 -NEWFTZ  C X  3 ) /SORT ( B*B+C*C >  * 
AUXANG : * AT AN2 (B.C) 3 

IF  A  >*  0.3999  THEN  ANGCG3 “3. 1416/2. -AUXANG 
ELSE 

BEGIN 

b:»asin<a>-auxang; 

C3“3. 1 4 153-ASIN(A> -AUXANG! 

IF  ABS ( B-TURANG )  <  ABS ( C-TURANG > 
THEN  ANGCG 3 «B 

else  angcg :-c; 

end: 

CGCX3 : “ OLDFT 11X3 -NEUFT 1 1 X  3 *COS ( ANGCG > 

♦NEUFT 1 C  Y  3  «S I N ( ANGCG  > J 
CGC Y3  3  “0LDFT1 CY3-NEWFT1 CX3*SIN( ANGCG > 

-NEWFT 1 C Y3  *CQS ( ANGCG ) I 
CGC23 : -OLDFT 1 CZ3-NEWFT1 CZ3 I 
OLDFTi: “NEWFT I!  0LDFT2 3 “NEWFT2 I 

{  COMPUTE  THE  TOTAL  ANGLE  AND  TOTAL  DISTANCE 
THAT  BODY  CENTER  HAS  MOVED  SO  FAR. 

C3 “COS(TANGCG) >  B3 “SIN( TANGCG ) > 

CGM0VEIX3  3 “CGMDVE C X 3 +CGCX 3 *C-CGC Y3*B : 
CGMOVEC Y3  3 “CGMQVEI Y 3+CGCX3*B+CGCY3*C: 
CGM0VECZ3 3 “CGMOVECZ3+CGCZ3 i 
TANGCG 3 “TANGCG+ANGCG* 

C3 «COS( TANGCG ) 3  B 3 «SIN< TANGCG ) * 

RESANG  3  * ANG-ANGCG ! 

ANGCG  3 “ANGCG* 180 ./3. 14153. 

{  ASSIGN  FOOTHOLDS  FOR  MIDDLE  AND  REAR  LEGS  FOR 
THE  NEXT  LOCOMOTION  CYCLE  > 

303  IF  LEGLIFT-S  THEN 

BEGIN 

FOR  13-  -S  TO  -3  DO 
BEGIN 
J3— i; 

FTHDC  4  >  XI 3 “PTFTHDt  J-2.X3 3 
FTHDC J  >  Y3  3 “PTFTHDC  J-2 . Y3  J 
FTHDC  J  >  Z3 3 “PTFTHDC  J-Z»Z3  * 

end; 

END 

else; 

LTGCCX3  3  -PTGCCX3  3  LTGCC Y3  3 “PTGCCY3  3 
PTGCCX3  3 -NTGCCX3  3  PTGCCY3 3 “NTGCC Y3 i 

<  FETCH  THE  NEXT  FOOTHOLD  FOR  A  FRONT  FOOT  > 

safe : -false; 

WHILE  NOT (SAFE)  00 
BEGIN 

newdata; 

footheight; 

turnangle; 

synthmotion; 

safeck; 

if  STOP  ■  1  THEN  SAFES “TRUE 

else; 
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<♦•£♦*) 


<  FILE:  F00T37. PAS  > 


/*  procedure:  footpath  */ 


/••••••««*«•«•••••••••••••»•••••••••••••••••••••••••»•••»••••••«•/ 

/*  programmers:  DENNIS  PUGH.  TED  CHANG  */ 
/*  DATE:  30-MAR-B2  */ 
/*  MODIFIED  BY  SHENG-JEN  TSAI  TO  SUIT  THE  FTL  LOCOMOTION.  •/ 
/*  */ 
/*  function:  calculates  foot  TRAJECTORIES  to  implement  */ 
/*  THE  BODY  RATES  COMMANDED  BY  BODY  MOTION  •/ 
/•  PLANNING.  */ 
/*  */ 
/»  user  guide:  the  calling  format  is:  footpath:  •/ 
/*  */ 
/*  */ 
/*  PROCEDURES  CALLED:  JSERUO.  RBADC  •/ 
/«  */ 
/•  GLOBAL  VARIABLES  •/ 
/•  referenced:  velx.  vely.  dpsi  •/ 

/*  MIDSTX >  MIDSTY.  MIDSTZ  */ 
/*  DT.  PERIOD.  SPERIOD  */ 
/•  PHASE.  RPHASE.  BETA  •/ 
/*  FOOTLIFT  */ 
/«  */ 
/*  modified:  xpd.  ypd.  zpd  */ 

/*  XRD.  YRD.  ZRD  */ 
/*  XFD.  YFD.  ZFD  */ 
/•  SUPPORT.  ZPTERM  */ 
/*  DXB.  DYB.  PSIC  */ 


PROCEDURE  FOOTPATH-. 

CONST  ATTSCALE  -  -0.7834:  /*  ATTITUDE  SCALE  FACTOR  */ 

ATTPOLE  »  1 .  :  /*  POLE  FOR  ATTITUDE  CONTROL  */ 

FTOTAL  «  310. O:  /*  TOTAL  VEHICLE  WEIGHT  (  LBS.)  */ 

PHRSCALE  >  -33333.3)  /•  POWER  SCALE  FACTOR  •/ 

VAR 

ALPHA, 

A.B.C, 

BH. 

CGX .  COY , 

DTI ,  DTZ. 

DXB1 .  DYBt , 

DXE .  DYE. 

LPHASE , 

PITCH.  ROLL. 

PPITCH,  PROLL. 

POWER . 

PSIC1 , 


/•  CRAB  ANGLE  WRT  LONGITUDINAL  AXIS  •/ 

/*  TEMPARORY  VARIABLES  */ 

/*  HEIGHT  OF  THE  BODY  */ 

/#  COORDINATES  OF  CG.  PROJECTION  •/ 

/*  BACKED  UP  TIME  FROM  MIDSTANCE 
/•  TOUCHDOWN  DISPL.  FROM  MIDSTANCE  */ 

/•  BODY  DISPLACEMENT  IN  EARTH  COORD.  •/ 

/*  LEG  PHASE  VARIABLE  •/ 

/*  ACTUAL  ANGLES  FROM  GYRO  */ 

/•  ACTUAL  ANGLES  FROM  PENDULUM  */ 

/•  HEXAPOD  INSTANTANEOUS  POWER  CONSUMP.  •/ 
/•  BODY  ANGULAR  DISPLACEMENT  OVER  DT  •/ 
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U.  R,  S. 

/* 

INIERMED.  FORCE  OPTIMIZATION  TERMS 

*/ 

STIME, 

/« 

TIME  AT  WHICH  DATA  FRAME  STORED 

*/ 

SUMX . 

SUMY. 

/* 

SUM  OF  FOOT  DISPLACEMENTS 

*/ 

SUMX2 , 

SUMY2  > 

/* 

SUM  OF  SQUARES  OF  FOOT  DISPLACEMENTS 

*/ 

SUMXY , 

/* 

SUM  OF  FOOT  DISPL.  CROSS-PRODUCTS 

*/ 

TIMEFP , 

/« 

REMAINING  TIME  IN  TRANSFER  PHASE 

*/ 

TRTIME , 

/* 

TOTAL  TIME  IN  TRANSFER  PHASE 

*/ 

VEL, 

/* 

MAGNITUDE  OF  VELOCITY  VECTOR 

*/ 

ZOFF. 

/* 

Z  POSITION  ERROR  FOR  ATT.  CONTROL 

*/ 

ZRTERM 

/* 

Z  RATE  OFFSET  FOR  ATTITUDE  CONTROL 

*/ 

:  real; 

FRSTCH, 

/* 

FIRST  A/D  CHANNEL  FOR  DATA  FETCH 

*/ 

1. 

/* 

LOOP  COUNTER 

*/ 

N 

/* 

SUPPORT  PHASE  LEG  SET  COUNTER 

*/ 

:  integer; 

TOPBLOCK 

/* 

TOP  A/D  CHANNELS 

*/ 

:  ARRAY is; 

PROCEDURE  JSERVOC  MOVE :  BOOLEAN  >!  EXTERNAL ! 

PROCEDURE  RBADC <  NOCHAN,  PRSTCHC  INTEGER!  SCALE:  REAL •  VAR  INDATA:  ARRAY18  ) 

external; 


function  deltatime:  real:  external; 

FUNCTION  ATAN2  <  Y ,  X  :  REAL):  REAL!  EXTERNAL! 


BEGIN  /*  F00T37  «/ 


/****»*•»»•*■»**#***•»**********■»»■*•*«•»*•»*•***«********»***********/ 

/*  EXPRESS  INCREMENTAL  BODY  DISPLACEMENT  IN  BODY  COORDINATES  */ 

/••*•••••*•••**••*••••••*•••*••«•»•••«•*••••••••••••••••••••••••/ 

/***  CALCULATE  INCREMENTAL  BODY  DISPLACEMENT  HRT  GROUND  ***/ 

psic  :«  dpsi  *  dt; 


/***  CALCULATE  MAGNITUDE  OF  VELOCITY  HRT  GROUND  *«*/ 
VEL  :*  VELX  *  VELX  +  VELY  *  VELY! 

IF  VEL  <>  0.0  THEN  VEL  :«  S3RT<VEL>! 


IF 


ABS(DPSI)  >  0.00001 
THEN 


BEGIN 

DXE 

DYE 

END 

ELBE 

BEGIN 

DXE 

DYE 

end; 


VEL/DPSI  «  S1N<PSIC>: 

VEL/DPSI  *  (1.0  -  COS(PSIC))! 


VEL  *  dt; 

o.o; 


/•••  ROTATE  DISPLACEMENT  VECTORS  TO  BODY  COORDINATES  ***/ 
ALPHA  :«  ATAN2 (  VELY,  VELX  >; 
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oxB  :> 

DXE  •  COS (ALPHA) 

-  OYE  *  SIN (ALPHA) S 

DYS  :■  DXE  •  SIN (ALPHA) 

+  DYE  *  COS (ALPHA)I 

RBADC (5.58,1, TOP8LOCK ) ; 

pitch: *topblocki73*attscale; 

ROLL : -TOPBLOCK  C  8  3 *ATTSCALE i 

<  ESTIMATE  THE  HEIGHT  OF  THE  BODY  > 

IF  ACTION  «  BODYMOTION  THEN 
BEGIN 

bh:-o. ; 

FOR  i:>l  TO  6  DO 
BEGIN 

bh:-bh+zpaci3; 

end; 

bh:«bh/6.: 

END 

else; 

/•••••••••••••••••••••••••••••••A/ 

/•  GENERATE  FOOT  TRAJECTORIES  •/ 

/*•••••••••••••••••*•••••••••••••/ 

FOR  I  :•  1  TO  6  DO  <  GENERATE  FOOT  COORDINATE  FOR  LEG  I  > 

BEGIN 

IF  NOT (  I  IN  SUPPORT  ) 

THEN  <  LEG  IN  TRANSFER  PHASE  > 

BEGIN 

A! *2 . *(FTHDI I , ZI-PTFTHDC I ,Z3 ) 

-4 .  *  <  FQQTLIFT-PTFTHDC I . Z3 ) ; 
b:«fthdci.23-ptfthdci.Z3-a; 

C:*PTFTHDtI ,Z3; 

IF  TPHASE  >  I.  THEN 
BEGIN 

xrdcii:-o.; 

YRDCIK-O.; 
zrdc 1 3  :«i . ; 
xpdci3:«xpaci3; 
ypdc i i : "Ypac 13 ; 

ZPDCI3;.ZPACI3+0.5; 

END 

ELSE 

BEGIN 

IF  <  LEG  AT  TOP  OF  TRANSFER  PHASE  AND  FORCE  NOT  YET  ZEROED  > 
(TPHASE  >0.5)  AND  (FZER0CI3  *  FALSE) 


THEN  <  UPDATE  OFFSET  FORCES  FUR  LEG  I  ) 

BEGIN 

frstch  :■  (  r  -  i  )  *  3; 

RBADC (  3.  FRSTCH,  FSCALE .  ZEROFORCE): 

FZEROC 1 3  :«  TRUE:  {  FLAG  THAT  LEG  I  IS  UPDATED  > 

ZPTERHC 1 3  :*  O.O;  {  INITIALIZE  ATTIT'J'c  -CRRECTION  TERM  > 

end; 

<  END  IF  > 
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/*«*  CALCULATE  TIME  LEFT  TILL  TOUCHDOWN  OF  THE  LEG  ***/ 

tihefp:-  < i .-tphase)*lifttime: 


/*•*  COMPUTE  DESIRED  FOOT  POSITION  ***/ 
IF  DT  <  TIHEFP 
THEN 


BEGIN 

XPDCI1 

YPDCI3 

ZPDCI3 

END 

ELSE 

BEGIN 

XPDCI3 

YPDCI3 

zpdcii 

end; 


XPDCI1  ♦  (FTHDCI.X3  -  XPDCI3) 
YPDtn  +  (FTHDCI.Y3  -  YPDCI3) 
A*TPHASE*TPHASE'»B*TPHASE+C; 


FTHDT i .  X3 ; 
FTHDCI.Y3I 
FTHDCI.Zi; 


<  END  IF  DT  > 


/***  COMPUTE  DESIRED  FOOT  RATE  ***/ 

XRDCI3  <  FTHDC I >  XI  -  XPDtll  )  /  TIMEFPS 
YRDtn  :=  <  FTHDC  I  *  Y  3  -  YPDCI3  )  /  TIMEFPJ 
ZRDtn  :=<2.*a*tphase*b>/lifttime; 

end;  <  ELSE  TPHASE  >  1.  > 

END  <  TRANSFER  PHASE  > 


ELSE  <  FOOT  IN  SUPPORT  PHASE  > 

BEGIN  # 

fzeroci]  :*  false;  <  flag  that  force  not  zeroed 
rsupportcik-io. ; 

/***  COMPUTE  ATTITUDE  CONTROL  VARIABLES  ***/ 

ZOFF  :«  -PITCH  *  XPAtn  ♦  ROLL  *  YPACI3: 

IF  (ATTITUDE  «  TRUE) 

THEN 

BEGIN 

zrterm:*attpole*zoff; 

ZPTERMC 1 3 : *ZPTERMC 1 3+ZRTERM*DTI 
IF  ABS (ZPTERMC I  3 )  >  ABS(ZOFF) 

THEN  ZPTERMC 1 3  ZOFF 

else; 

end 

ELSE 

BEGIN 

ZPTERMC 13  :»  o.o; 

ZRTERM  :■  o.o; 
end; 

<  END  IF  ATTITUDE  > 

/*•*  COMPUTE  DESIRED  FOOT  POSITION  **•/ 

XPDCIJ  :■  PTFTHDC I »X3 ; 

YPDCI3  >  PTFTHDC I.Y3J 

<  ADJUST  THE  HEIGHT  OF  THE  BODY  IF  NECESSARY  > 

IF  ACTION  »  BODYHOTION  THEN 
BEGIN 

IF  BH  <  13.  THEN 


DT/TIMEFP; 

DT/TIMEFP; 


THIS  CYCLE  > 


269 


r  u  L  1  J  , 


4-rwi.  i  •  *UT 


ELSE  IF  BH  >  20  THEN 

zPDcn:»2PDcn-o.s/n.*DT 
END  else  ZPDcnr.zpocn: 

ELSE 

2POCI3  :•  PTFTHDC  I .  Z3*  ZPTERMtn; 
/***  COMPUTE  DESIRED  FOOT  RATE  ***/ 

XRDtn  :•  -uelx  *  dpsi  *  ypdcij: 

YRDCI3  -UELY  -  DPSI  *  XPDCI3I 

zrdcij  :«  zrterm: 

end:  /*  SUPPORT  PHASE  */ 


end:  /*  FOR  1  */ 


'  ’T**********#*t**tt#t#tMtt##f#tt##tt/ 

/*  COMPUTE  OPTIMAL  FORCE  SETPOINTS  */ 

I  **********  ********##♦#•**  **•»****«.  JLJL  M.M.  t 


n  o; 
sunx  :■  o.o; 
sumy  o.o; 
sumx2  :«  o.o: 
suny2  :■  o.o: 
sumxy  :■  o.o; 


FOR  I  :«  1  TO  6  DO 

IF  I  IN  SUPPORT  THEN 
BEGIN 

sumx  :«  sumx  ♦  xpacm; 

SUMY  :«  SUMY  ypacii; 

SUMX2  :*  SUMX2  ♦  XPACI)  *  XPACI 1 1 

sum y2  :*  sumy2  ♦  ypacii  *  YPAtn: 

sumxy  :■  sumxy  ♦  xPAtn  *  ypacii: 

n  :•  n  ♦  i; 

end:  <  IF  I  IN  SUPPORT  > 

<  END  FOR  I  > 


R 

Q 

s 


< SUMX  •  SUMY2  -  SUMXY  «  SUMY) 

(SUMY  -  SUMXY  *  R)  /  SUHY2 ! 

N  -  ((SUMX  *  SUMYZ  -  SUMXY  *  SUMY) 


/  (SUMX2  *  SUHY2  -  SUMXY  *  SUMXY): 


*  R  +  SUMY  *  SUMY)  V  SUM Y2 ! 


CGX  :■  -SIN(  PITCH  )  *  MIDSTZ; 
CCY  :»  SIN(  ROLL  )  •  MIDSTZ; 


/***  COMPUTE  FOOT  FORCE  SETPOINTS  ***/ 

FOR  I  :«  I  TO  6  DO 
BEGIN 

xfdcij  :•  o.o: 
yfdcii  :■  o.o: 


IF  I  IN  SUPPORT 
THEN 


END 


IF  OPTIMIZATION  •  TRUE 

THEN  ZFDCIJ  I*  (  1  -  0  #  (  YPATI3 

-  R  *  <  xpacii 

ELSE  ZFDCI3  :«  FTOTAL  /  N 

ELSE 

ZFDtn  o.o; 

END!  <  FOR  I  > 

JSERVQt  MOVE  >:  <  CALL  SERVO  ROUTINE  > 

;  /*  FOOTPATH  */ 


-  COY  > 

-  CGX  )  )  *  FTOTAL  /  S 
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CHECK. PAS 


(**£♦*) 

<  FILE  CHECK. PAS  > 


/*******************#*********************************/ 

/•  PROGRAMMER  I  SHENG-JEN  TSAI  */ 
/*  DATE  I  APRIL  20.  1983  */ 
/•  FUNCTION  I  GIVEN  A  FOOTHOLD  (KG.YG.ZG)  FOR  A  LEG.  */ 
/♦  CHECK  IF  THE  LEG  CAN  REACH  IT  WITHOUT  */ 
/*  EXCEEDING  THE  KINEMATIC  MARGINS.  */ 
/*  */ 
/*  FUNCTIONS  CALLED  I  ATAN2.  ASIN.  SIGN  */ 
/«  */ 
/•  USER  GUIDE  I  >PAS  CHECK-CHECK  •/ 
/#  >MAC  CHECK -CHECK  */ 
/*  */ 
/••••••••«••••••••••«•••#••*••••*••••*••••••••••••••••/ 


PROCEDURE  CHECK <XG.YG.ZG  I REAL 5  VAR  SAI . TH1 . TH2 1  REAL ) ) 

CONST  Ll-12.56;  L2-17.00)  L3-0.) 

L4-  2.3:  L5-2 . 463. 

XHIP-22 . 73  )  YHIP-8 .362  5 


uar  s.x. y.z. a.b.c.d.eireal; 

FUNCTION  ATAN2(Y.X  I  REAL)  I  REAL)  EXTERNAL) 
FUNCTION  ASIN (VALUE  I  REAL)  I  REAL)  EXTERNAL) 
FUNCTION  SIGN( X  I  REAL)  IREAL)  EXTERNAL) 


BEGIN 

ZI-ZG) 

y:«yg)  x:-xg) 

SAI : -ATAN2CX.Y) ) 

S:-SIN(SAD)  C:-COS(SAI)) 

SAI :«SAI*180./3. 14139) 

IF  S-0.  THEN  AI-O. 

ELSE  A:-(X-L4*S*L3*C)/B) 
a:«a«A) 

B)-A+Z*Z  1*L1 -L2*L2-L3*L3 I 
d:-li*l:  -3*l3) 
d:-sqrt(D> ) 

D:-2.*L2*D) 

b:-b/d) 

IF  ABS(B)  >  1.  THEN  B:-SIGN(B) 
ELSE ) 

b: -asin(B) ) 

E:*ATAN2(L3.L1 > ) 

TH2I -B-E) 

S ) *S IN ( TH2 ) )  c:-C0S(TH2>) 

TH2)-TH2*lB0./3. 141385 
a:-l3+l2*c;  b:»li*l2»s) 

c: »a*a*b*bi 
C:-SQRT(C) ) 

c:-z/c: 

IF  ABS(C)  >  1.  THEN  C:-BIGN(C) 
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end; 


else: 

c:»-asxn<c) ; 
d:»atanz< a,b;  : 
thi :-i>d: 

THl :«TH1* ISO. /3. 14139: 
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CENTER. PAS 


<  file:  center. pas  > 


/«  programmer:  sheng-jen  tsai  •/ 

/*  DATE:  APRIL  20,  1S63  */ 
/*  FUNCTION:  GIVEN  (Xl.Yl.Zl.IXl.IYl)  AND  <X2. Y2,Z2. 1X2. IY2> .*/ 
/*  COMPUTE  THE  LOCATION  OF  THE  LENS  CENTER  WITH  */ 
/#  RESPECT  TO  THE  IMAGE  PLANE.  */ 
/#  •/ 
/•  PROCEDURE  CALLED:  NONE  •/ 
/*  */ 
/•  USER  GUIDE:  >PAS  CENTER-CENTER  •/ 
/*  >MAC  CENTER  *  CENTER  */ 
/•  >TKB/FP/CP«CENTER. Cl , 1JPASL IB/LB  •/ 
/*  >RUN  CENTER  */ 
/#  */ 


PROGRAM  CENTER! 

LABEL  10 I 

TYPE 

MATR IX* ARRAY C } . .4, 1 . .4]  OF  REAL; 

VECTOR*ARRAYC 1 • .31  OF  REAL; 

CONST 

F-S.3E-3; 

DX-4.SE-3: 

DY-3.8E-S: 

NX-248; 

NY-244; 

A3— 7.0733S4E2I  A2-B.B4817EI I 

A1--3.2127E-I ;  A0-B.181SE-4; 

B3--7. 8S33SE2 :  B2-4 . 78627E1 ; 

B1--1 . 18877E-I ;  B0--3.397BE-3, 

VAR 

tmatrix: matrix; 

C21F.C22F.V21  .  V22  ,  VI  ,  U1  ,  U2 ,  D :  VECTOR; 

1 .  X I  OFF .  Y 1  OFF ,  X2DFF ,  Y2 OFF .’  INTEGER ; 
X.Y,Z,AC,BC.A,B,C, IR,R,XIM2,YIM2.ZIM2. 
K1.K2,K3,LX.LY , ANG, XIM1 , YIM1 , Z1M1 , 

XC,YC,X1,Y1 ,Z1 ,X2, Y2,Z2,XP1 ,YP1 .XP2,YP2:REAL; 


274 


i 


BEGIN 


10: 


WEADLN^XPl^ypI.xpz,^^  C00WlNATeS  XPJ.YP1.XP2.YP2' 

WPITELNI 'INPUT  Xl.Yl.Zj',; 

REAOLN<  XI . YI . 21 J { 

SciIELN< 'INP°r  X2.Y2.22'); 

PEA0LN<X2. Y2.22) ; 

Dill : “X2-X1 ;  Bf2j;»Y2-Yi; 

lx:«nx*dx;  ly:»ny*dy;  0C31*  22  21 ’ 
xim :»(xpj-o.3)*dx; 
yihi:«<ypi-o.s>*dy; 
zihi  :•<>.: 

<m  i  j:»xihi-xi: 

W1C23:»yIN1-yj; 

V1C31:*2Ini-2i • 

xi«2:»(xp2-o.3»*ox; 
yui2:.<yp2_o,3j*oy; 
zim2:.o. ; 

^22in:.xiM2-x2; 

y22C23 : ■YIM2-Y2* 

W22C31:.2im2-Z2: 

B:«soBT(8KCll+wlC21*Vit23+wtt3,#wlt33; 


B 


END. 


c:»safft<c*;22r,I*V22CZJ*VZ2t23*VZ2C33*W22t3Jf 
pop  i : - I  To  3  oo 
begin 

uunr.vtc  ij/b: 

END?tn:"V22clJ/c’’ 

jilsiKJsaijiiRisigiJijsss!,,. 

BC:.(K2#XI-K3)/fl.-K2#K2»; 

xc:«x/dx;  yc:«y/oy;  ,r'10,  -z.iok 

"'K&S5S ?"•••«•••••’• '  ych.'ji; 

y;»8C*U2C21*Y2; 

Z."»8C*U2C3J*Z2; 

GOTO  so; 
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ROOT4.PAS 


<**E+*) 


/••••••••••••••••••••••••••••••••••••»••••••••••••*••«•••••••/ 

./*  FILE:  R00T4.PAS  */ 

/*  programmer:  sheng-jen  tsai  •/ 

/*  DATE:  MARCH  12,  1983  */ 

/*  */ 

/*  FUNCTION:  TO  COMPUTE  THE  POSITIVE.  REAL  ROOTS  OF  A  4TH  */ 

/*  ORDER  POLYMIAL  BY  NEWTON-RAPHSON  METHOD.  THE  */ 

/*  DESIRED  ROOT  R  MUST  BE  GREATER  THAN  THE  */ 

/•  INITIAL  VALUE  IR.  •/ 

/*  */ 

/•  PROCEDURES  CALLED:  NONE  •/ 

/*  */ 

. . **#************.«***«******.****.*******.***/ 


procidur*  ROOT ( K4 . K3 . K2 . K 1 . KO:r*al ! 

var  r.ir:read; 

VAR 

POFR . PDOFR . KEY , RNEXT : REAL i 


BEGIN 

key:*i.; 

r:*ir: 

pofr:*i.; 

WHILE  ABS(POFR)  >*  l.OE-6  DO 
BEGIN 

pofr: *-k4*r*r*r*r-k3*r*r*r-kz*r*r- ( k 1-: > *r-<ko+ir ) ; 

PDOFR: *-4 . *K4*R*R*R-3. *K3*R*R-2 . *KZ*R- (K 1-1 ) ; 

key:*pofr/pdofr; 

r:»r-key; 

end; 

end: 
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FOVF.FTN 


ccccccccccccccccccccccccccccccccccccccccccccc 
c  programmer:  shenj-jen  tsai 

c  date:  march  12.  isas 

C  FUNCTION  COMPUT  THE  FIELD  OF  VIEW  OF 
C  BOTH  CAMERAS  AS  A  FUNCTION  OF 

C  TILT  AND  ROLL  ANGLES  AND  PLOT 

C  THE  RESULT  IN  THE  HP  PLOTTER. 

C 

C  SUBROUTINES  CALLED:  INITZ.  HPPLOT,  STOVER 
C 

C  USER  GUIDE:  FOVF/FP/CP-F  _iVF » HPPLOT , 

C  t 1 • 1 3F4POTS/LB 

C  / 

C  COMMON- I OP AGE. -WR 

C  // 

C 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

DIMENSION  CENTER (3) , UR ( 3) . UL ( 3 ) , URB (3 ) , ULB < 3 ) 
DIMENSION  T<4.4> ,0(3) 

DIMENSION  IM  ( 2 , 30  >  , ICA1 <2.5> . ICA2<2,5) 

REAL  K,LX,LY,LZ»LRC3) . LL(3) . LRB ( 3  > . LLB ( 3  > 

GENERATE  DATA  ARRAY  IM  FOR  THE  HEXAPOD  BODY 
TO  BE  PLOTTED  IN  HP  PLOTTER 

IM( 1 > 1 ) -350 
IM ( 2 . 1 )-332 
IMU. 21-425 
IM<2. 21-332 
IM<1 ,31-425 
JM<2. 31-216 
IMU  ,41-350 
IM<2, 41*216 
IMU, 51-425 
IM(2 , 5 1 -21S 
IMU, 61-425 
IM(2, 61-100 
IM< 1,71-350 
IM  <2 » 71  *  100 
IM< 1 ,8) - IM( 1,6) 

IM<  2 , 6 1  *  IM(  2 , 6 1 
IMU. 91-575 
IM<2, 91-100 
IMU, 101-650 
IH<2, 101-100 
IM( 1,11) -IM( 1,91 
IH(2,lt)-IM(2,9) 

IM<1, 121-575 
IM<2, 121-216 
IM  < 1 . 131-650 
IM<2. 131-216 
IMU  .  141-IMU  .  12) 

IM ( 2 , 1 4 1 « IM ( 2 , 12) 

IMU,  151-575 
IH<2, 151-332 
IM  < 1 . 161-650 
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u  U 


IH!2» 16>»332 
imi.l7>«IM!l»2> 

IH<2. 17) « IH! 2  >  2  > 

N*0 

00  11  1*1 >2 
DO  11  J-1.5 
1CA1 ( I • J ) *0. 

ICA21 I . J)*0. 

11  CONTINUE 

C  CLEAR  THE  HP  SCREEN  AND  PLOT  THE  HEXAPOD  BODY 

CALL  1NITZ 

CALL  HPPLOT ( XCA1 • 5,2> 

CALL  HPPLOT I ICA2 >S>3) 

CALL  1N1T2 

CALL  HPPLOT! IM. 17,1) 

1  NRI TE ( 5 , 5) 

5  FORMAT ! ' INPUT  PITCH  AND  ROLL  ANGLES') 

READ! 5 > • )  ANG1 , ANG2 
ANG2«ANG2/180.*3. 1416 
ANQ1 *ANGl / 180 . *3 . 1 4 IS 
N-N*l 
NT  *0 

F-S.5E-3 
PX-4.6E-3 
PY-3.6E-3 
DX—0.13 
DY-0.13 
D2«-0.32S 
LX-0.03 
LY-O. 

L2*0. 

COMPUTE  THE  FIELD  QF  VIEW  OF  CAMERA  1 
AND  CAMERA  2 

CENTER! 1>-118*PX 
CENTER! 2 ) *  1 1S*PY 
CENTER!3)*-F 
4  UL! 1 ) *0, 

ULC2) »0. 

UL ! 3 ) *  0 . 

UR! 1 >*24B*PX 
UR ! 2  >  *  0 . 

UR!3) *0. 

LL< l ) *0. 

LL ! 2 ) *244»PY 
LL!3)*0. 

LR! 1 )»Z4B*PX 
LR ( 2 ) *244*PY 
LR! 3) *0 . 

NT-NT*l 

DO  10  1-1,3 

UL ! I ) -UL ! I ) -CENTER ( I ) 

UR! 1 ) -UR! I ) -CENTER ! I ) 

LL ! I ) -LL  < I ) -CENTER ! Z ) 

LR ! 1 > -LR ! I ) -CENTER ! I > 


/ 
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10  CONTINUE 

r<i,n«o. 

TU.2>—  COS(ANGl) 

T  ( 1 »  3 ) «  S I N  <  ANG 1 > 

T(2.1)-C0S(ANG2> 

7(2.2) --SIN ( ANGI >  *S1N (  AND2 ) 

T(2>3)--CQS<  ANG1 ) *SIN( ANG2  ) 

T(3. 1 ) -S1N( ANG2> 

T(3 • 2 ) »SIN ( ANGI >*C0S(ANG2) 

TO.  3  >  * COS ( ANGI  )  *COS  <  ANG2  ) 

DO  40  1-1 .3 

ULB(n»ra,n«uLn>+T<i,2>*ULC2)+Tc.3)*uLO) 

URB( I )-T( I, 1 )*UR(1)+T( I,2)*UR(2)+T( I ,3)*UR(3) 

LLB( I >*T(3 ,l)*LL(l > *7 ( I . 2 > *LL ( 2>*T ( I , 3 )*LL (3 > 

LRB  (I)-T(I.l)  #LR  ( 1 )*T( I  ,  ,?)*LR  (2) *T( I , 3 )  *LR  <  3 ) 

40  CONTINUE 

0<  1 )-LX*COS(ANat )*LZ*SIN.ANG1)+DX 
0(2) »LX*SIN(ANG2)*SIN( ANGI ) *L Y*C0S ( ANG2 ) 

1  -LZ*SIN( ANG2 ) *C0S( ANG1 ) +07 

0(3) *~LX*SIN{ ANGI ) *C0S ( ANGZ ) +L Y*SIN( ANG2 ) 

1  *LZ«C0S( ANG2  >*C0S( ANGI >  +DZ 
K*(0. 7-0(3>)/ULS(3> 

UL8( l ) *Q< l )*ULB( 1 )*K 
ULB (2) *0(2 )+ULB ( 2) *K 
ULB <3 ) *0  <  3 ) +ULB ( 3  >  *K 
K«<0. 7-0(3)  WURBO) 

URB(1 ) *0(1 l+URBC 1 )*K 
URB (2) -0(2 ) *URB ( 2  >  *K 
URB ( 3 ) -0 ( 3 ) +URB ( 3 ) *K 
K«<0. 7-0(3) >/LL8<3> 

LLB ( 1 ) *0 ( 1 ) +LLB ( 1 >*K 
LLB (2 ) *0 ( 2 ) *LLB <  2 ) *K 
LLB ( 3 ) *0 ( 3 ) +LLB ( 3 )  *K 
K* (0. 7-0(3) l/LRB (3) 

LRB ( 1 ) *0 ( l ) *LR8 ( l ) *K 
LRB ( 2 ) *0(2) +LRB ( 2) *K 
LRB (3 ) »Q<3) *LRB( 3  >  *K 
PI TCH- ANGI *180. /3. 1416 
ROLL -ANGZ* 180. /3. 1416 
WRITE(5.30>  PITCH. ROLL 

50  FORMAT ( // . 'ANGI » ' . F8. 4 , '  ANG2«',F8,4> 

HRITE(5.S0)LRB( 1 ) .LRB (2) ,LRB(3> 

60  FORMAT  < / . 'LRX* ' .F8 . 4 , '  LRY-',F9.4,'  LRZ»'.F8.4> 

WRITE (5.70)  LLB ( 1 ) . LLB ( Z ) . LLB ( 3 ) 

70  FORMAT (/. 'LLX*  '  .F8. 4 ,  '  LLY«',F8.4.'  LLZ-',FB.4) 

WRITE (5 . 80)  URB ( 2 ) . URB ( 2 ) . URB ( 3 ) 

60  FORMAT  < / . 'URX* ' ,F8. 4. '  URY- ' , FS. 4 .  '  URZ*',F8.4> 

WRITE  (5.  BO)  ULB  (  1  •  UL8  ( Z )  .  ULB  (  3  ) 

80  FORMAT  (/.  'ULX»  '  ,F8. 4.  '  ULY«',F8.4.'  ULZ«',F8.4> 

FAC-200. 

IF  (NT  .EG.  2)  GO  TO  7 
•  I CAi ( 1 . 1 ) -ULB ( 2 >  *FAC*500 
ICA1 (2.1 )-ULB( 1 )*FAC+332 
I CAI (1.2) -URB ( 2 ) *FAC+500 
ICA1 ( 2 . 2 ) -URB ( 1 ) *FAC*332 
I CAI ( 1 .3)»LRB(2)*FAC*500 
ICAl (2.3) -LRB ( 1 ) *FAC+332 
ICA1(1.4)«LLB(2)*FAC*500 
ICAl (2.4)»LLB( 1 >*FAC*33Z 
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i 


ICA1 ( 1 ,3)*ICA1 ( 1 . 1  ) 

ICA1 (2.3) -ICA1 <2. 1 ) 

PLOT  THE  FIELD  OF  VI EH  OF  CAMERA  1 

CALL  STOVER(Z) 

CALL  HPPLOTC ICA1 .3.2) 

GO  TO  9 

ICA2< 1.1) *ULB ( 2  >  *FAC+500 
ICA2 (2.1 ) >ULB ( 1 >*FAC+332 
1 CA2 (1.2) ■URB <  2 ) «FAC+SOO 
ICA2 (2.2) «URB ( 1 ) *FAC+332 
1CA2 (1.3) *LRB ( 2  >  *FAC+500 
ICA2(2.3)»LRB(1) *FAC+332 
ICA2 (1.4) *LLB ( 2 ) *FAC+500 
ICA2 (2.4) *LLB ( 1 ) *FAC+33Z 
ICA2 (1.5) • ICA2 (1.1) 

ICA2 (2.3>*ICA2(2. 1 ) 

PLOT  THE  FIELD  OF  VIEW  OF  CAMERA  2 

CALL  STOVERO) 

CALL  HPPLOT ( I CA2 .3.3) 

IF  (NT  .EQ.  2)  GO  TO  3 
ANGZ«-ANG2 
DY»-0.2 

CENTER ( 1 ) *1 13*PX 
CENTER(2)*112*PY 
GO  TO  4 

IF  (N  .LE.  20)  GO  TO  1 
STOP 
END 


POLYFT  «  PAS 


/«•«•••••••••••••••••••••••*••»*••••••*•••••••••••••••••••••••••••/ 

/«  FILE:  POLYFT. PAS  */ 

/•••••A***********************************************************/ 
PROGRAM  POLYFT! 

/••••••***••••*•*•••• . •••••*••*•••.••••••*•••••••••••••••»••••/ 

/*  programmer:  s.  j.  tsai  */ 

/«  DATE.'  JULY  e  1982  */ 

/*  function:  */ 

/*  THE  FUNCTION  OF  THIS  PROGRAM  IS  TO  FIT  A  SET  OF  POINTS  BY  */ 

/*  USING  A  POLYNOMIAL.  THE  NUMBER  OF  POINTS  TO  BE  FITTED  IS  50  */ 

/*  OR  LESS, AND  THE  DEGREE  OF  THE  FITTING  POLYNOMIAL  IS  LIMITED  TO*/ 

/*  19  OR  LONER.  HOWEVER  >  THE  R I  STRICT I ON  ON  THE  NUMBER  OF  DATA  */ 

/«  POINTS  CAN  BE  EASILY  REMOVED  BY  CHANGING  THE  DIMENSION  OF  */ 

/*  ARRAYS  USED  IN  THIS  PROGRAM.  */ 

/*  */ 

/*  THE  PARTICULAR  ALGORITHM  EMPOLYED  IN  THIS  PROGRAM  TO  FIT  */ 

/*  POINTS  IS  BASED  ON  "ORTHOGONAL  POLYNOMIALS" , AND  THE  ERROR  IN  */ 
/*  FITTING  IS  MINIMUM  IN  THE  LEAST  SQUARE  SENSE.  THE  ALGORITHM  */ 
/*  IS  OUTLINED  AS  FOLLOWS.  */ 

/*  */ 

<  PtN-M  3  (X)  *  < X-BETACN1 ) *PCN1 ( X ) -GAHACN1*PCN-1 I  ( X  >  > 

<  BETACN1 «SUM( P  CN 1 <X!*PCNI(X)*X) /SUM  <PCN1(X)*PCNI(X) )  > 

<  GAMArNI*SUM<PCNJ(X>«PCNHX> ) /SUM t PCN-13 ( X ) *PCN- 1 H X ) )  > 

/*  PC03  <  X ) *0 .  pri3(X)*l.  */ 

/*  WHERE  PCNJ(X)  IS  THE  NTH  ORTHOGONAL  POLYNOMIAL.  */ 

/*  */ 

/#  THE  DATA  POINTS  TO  BE  FITTED  ARE  STORED  IN  AN  EXTERNAL  */ 
/*  FILE  “CLEN.DAT" .  IT  IS  THE  RESPQSI8ILITY  OF  THE  USER  TO  */ 

/*  CREAT  SUCH  A  FILE  BEFORE  RUNNUIG  THIS  PROGRAM.  */ 

/*  */ 

/*  USER'S  GUIDE:  */ 

/*  >PAS  POLYFT "POLYFT  */ 

/*  >MAC  POLYFT" POLYFT  */ 

/*  >TKB  POLYFT /FP/CP* POLYFT .  1 1  . 1 3PASLIB/LB  */ 

/*  >RUN  POLYFT  */ 

/*  */ 

/••*»*•••••«*•*•««•••*•••••••••••«••••••••••«•••*•••••••••••••••••/ 


TYPE 

ARRAY80»ARRAYC 1 . .S030F  REAL; 

INDEX" 1 . .BJ  : 

MATRIX"  ARRAY!  INDEX  >  INDEX  I  OF  REAL,' 

ITEM" 

RECORD 

x:real; 

y:real 

end: 

PATH-FILE  OF  ITEM'. 

VAR 

ACOE ,  BETA ,  GAMA  ,  X .  Y .  F A  ,  FB ,  COE ,  YCAL  :  ARRAY80 ,' 

YY. STD,  SUM, SUMX,  SUMY, SUMZ,  PD, FF. DIF. PA.PB,  PC :REAL,' 

I, j.k: index; 
p '.matrix: 

n.mp. mi. size: integer; 
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point:iteh: 

clenkpath; 


BEGIN 

/*•*****«*•***********•********************************/ 
/*  READ  IN  THE  DATA  POINTS  TO  BE  FITTED  FROM  THE  */ 

/*  EXTERNAL  FILE  “CLEN1.DAT"  */ 

/••••••••••••••••••••••••••••••••••««••••*••••••«••••••/ 


size:«i  ; 

MRITELN< 'INPUT  THE  NUMBER  OF  POINTS  TO  BE  FITTED  .  Ml'): 
READLN<M1  )  ! 

RESET (CLEN1 .  'CLEN1.DAT',  'CLEN1 . DAT '. SIZE > : 

FOR  i:*l  TO  Ml  DO 
BEGIN 

point:«cleni a; 
xcn:*poiNT.x: 
yci3:*point.y; 

GET ( CLEN1 > l 

end; 

URITELNl 'INPUT  THE  DEGREE  OF  POLYNOMIAL  .  N')J 
READLN(N) ; 

np:*n+i ; 


/••••••••«•••••••*••••••••••••••••••••••/ 

/*  BEGINNING  OF  THE  ALGORITHM.  */ 

/•••••«•••••••••*•••••••••••••••••••«•*•/ 


sumy:«o.:  sumx:*o.; 

FOR  JI«1  TO  Ml  DO 
BEGIN 

sumy:*sumy«-ycj3: 

SUMX:*SUMX+XC  JK 

facjkm.:  fbcjk«o.: 

end; 

coEcn:«suMY/Mi; 

BETAtn:*suMX/Mt; 
gamac i ] : *0. ; 

FOR  j:»1  TO  Ml  DO 
BEGIN 

ycalcji:*coeci j; 
end; 
pd:«mi ; 

IF  NP  >*  Z  THEN 
BEGIN 

FOR  I I«2  TO  NP  DO 
BEGIN 

8umy:«o.:  sumx:*o.;  sumz:*o.: 

FOR  j:»l  TO  Ml  DO 
BEGIN 

IF  I  <■  2  THEN  FF:=XCJJ-BETAC!-1 J 
ELSE  FF:»(XCJ3-BETA:I-1])*FACJ1 

-gamaci-ii*fbcj:: 

FBCJ3:*FAUJ; 

FAC  J  3 ; *FF ; 

pb:«ff*ff: 
pa;«pb*xc ji: 
pc:-ff*yc jj: 
sumy:«sumy*pa: 
bumx: »sumx+pb: 


sukz:  3l  z+pc: 
end:  /•  END  J  */ 
beta  1 1 : : «sumy/sumx : 
gamaci j:«sumx/pd; 

COEC I  3 : *SUMZ/SUMX! 

pd:=sumx; 
sum:*o. : 

FOB  j:»l  TD  Ml  DD 
BEGIN 

YCALCJ  3 : *YCALC  J1+C0EC I 3*FAC  J  3  I 

dif:«yc jj-ycalc jj ; 
sum:»sum-*-dif*dif: 
end:  /•  end  j  •/ 
std:*sgrt(sum/mi > : 
end:  /•  end  i  •/ 

END  /*  END  THEN  •/ 

ELSE 

BEGIN 

yy:«coeci j; 

-sun:*o. : 

FOR  I : - 1  TO  Ml  DO 
BEGIN 

DIF  I «YC I 3-YY ; 

sum:*sum*dif*dif; 

end: 

std:*sgrt<sum/mi > : 
end: 


/••••••«•••••••••••••••••••••••••*•«•••••*•••••*»«•«•••••/ 

/*  COMPUTE  THE  COEFFICIENTS  OF  THE  POLYNOMIAL  */ 

/*•**********♦*******************•******•****************/ 


FOR  k:-i  to  np  do 
BEGIN 

pci>kj:*o.: 

end: 

pci.n:«i.; 

for  i:*2  TO  NP  DO 
BEGIN 

FOR  j:»l  TO  I  DO 
BEGIN 

IF  I«Z  THEN  PCI,Ji:«-BETACI-n*PtI-l,Jl 
ELSE  PCIrJi:»-BETAtI-n*PCI-l,  J3 

-gamaci-ii*pci-z.  ji: 

IF  J  >■  2  THEN  PtI.Ji:*PCI.J3+PCI-l, J-13 

else: 

end:  /*  end  j  */ 
end;  /•  end  i  •/ 
for  j:«i  to  np  do 

BEGIN 

acoecji:»o. ; 

FOR  I : - 1  TO  NP  DO 
BEGIN 

AcoErjj:»AcoEm+coEcn*pci.  ji: 
end;  /•  END  I  */ 
end;  /•  END  J  •/ 

MRITELN< 'THE  DEGREE  OF  POLYNOMIAL  IS'.NK 
MRITELN( 'THE  STANDARD  DEVIATION  STD: 10)1 
HRITELNI  'THE  COEFFICIENTS  OF  THE  POLYNOMIAL  ARE  I'll 
FOR  i:«l  TO  NP  DO 
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BEGIN 

MRITELN( 'A'. JI2. 

end; 

END. 


,AcoEcn:so> 
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TR I ON. PAS 


S  (*•£♦*) 


/*********•***********************♦***«*******«#*****•**•/ 

/*  file:  trion.pas  */ 
/*  programmer:  shenj-jen  tsai  */ 
/*  date:  march  12.  iss3  •/ 
/*  */ 
/*  function:  to  COMPUTE  THE  3-DIMENSIONAL  coordinates  */ 
/*  (XT.YT.ZT)  OF  AN  OBJECT  POINT  FROM  ITS  Z-D  */ 
/*  IMAGE  COORDINATES  < XPI . YP1 , XP2 . YP2 >  BY  •/ 
/*  TRIANGULATION.  */ 
/*  */ 
/*  PROCUDURES  CALLED:  ROOT  */ 
/*  GLOBAL  FILE:  GBLTRI.PAS  */ 
/*  */ 
/•  USER  guide:  >PAS  TRION«GBLTRI .TRION  •/ 
/«  */ 
/•••••••••••••••••••••••••••••••••••••••••••••••••••••••ft/ 


PROCEDURE  TRIONi 
TYPE 

VECTOR-ARRAY! 1. .31  OF  REAL! 

CONST 

f-s.se-g: 

DX-4.SE-5! 

dy«3.be-s: 

NX*24B: 

NY-244! 

UAR 

C21F.C22F.U21.V22.U1.U1 -U2.D: VECTOR; 

I .X10FF.Y10FF.X20FF, YZOFF: INTEGER! 

AC.BC.A.B.C. IR.R.PCTERR. 

K 1 . K2  ,  K 3 . XPCET , YPCET . ZPCET . LX . L Y . ANG , 

rx,ry,rz:real: 

A4.A3. A2.A1 , AO .B4.B3.B2.B1 >BO! REAL ! 

XI .Y1 .21 .X2,Y2.Z2:REAL; 

PROCEDURE  ROOT ( K 4 ,  K3 , KZ .  K  l ,  KO : REAL ! 

var  r,ir:real>:  external: 

FUNCTION  ATAN2<  Y , X : REAL  > : REAL !  EXTERNAL! 

BEGIN 

A4! *-6 . 420006E3 !  A3 : *7 .B77979E3 :  AZ: « 1 . S97816E1 ! 
A 1 : — 1 . 1 1 438 1 E- 1 :  AO : « 1 . 1 793E  3E-4 ! 

B4 :■ -8. 432071 E5 ! 

B3:*8.959416E3;  BZ  l  -3 . 917839  !  B1  :  — 6 . 724131E-2 : 
BO: *9. 380797E-3I 

a:«(xpi-ii9)«dx:  b:«<ypi-iib>«dy: 

I R : ■SORT  <  A*A+B*B ) I 
IF  i IR  >  2 . 5E-3 )  THEN 
BEGIN 

anq: •atan2<b.a> : 

ROOT (A4. A3, A2.AI ,AO.R. IR) ! 

a:«r«cos<ang) ;  b: «r*sin<ang) : 
end 
else; 
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vuij:-a; 

wi£2j:*b; 

vic33:*f; 

a:«(xp2-iii  )«dx;  b:«<  yp2-u3>*dy: 

ir:»sqrt<a*a+b*b>  ; 

IF  <IR  >  2.5E-3)  THEN 
BEGIN 

ang:*atan2<b, a> ; 

ROOT (B4  >  B3  >B2  >  B1 » BO  >  R • IR ) ! 

a:*r«cos(ang) ;  b:«r*sin<ang> : 
end 
else; 

v22Cii:>a; 

V22C2K*B: 

v22I3i:=f; 

C22FCIi:=DX*112: 

C22FC23 : *DY*1 131 
C22fc3i:*-f; 

FOR  I : -1  TO  3  DO 
BEGIN 

V21Cn:«TMATRIXCI  ,  13*V22C13+TMATRIXCI  ,23*V22t23 
♦TMATRIXCI ,3I*V22£3i: 

C21FCU:«TMATRIX£I , 1 3*C22FC l 3+TMATRIXC I ,23*C22F£23 
♦TMATRIXCI ,33*C22FC33+TMATRIXC I ,43 ; 

end; 

BC-VICl 3*VlC13*VlC23*ViC2:+VlC33*VlC33; 
b:*sqrt(B)  ; 

C:«V21C13*V21£1 3+V21C23*V21 C23+V21C33*V21C33; 
OSQRT(C) ; 

DCn:«TMATRIXCl  .43;  DC 2 3  :  *  TMATR I X  C  2 ,43; 

DC33 : *THATRIXC3»43  ; 

FOR  i:«l  TO  3  DO 
BEGIN 

uun:«vui3/B; 

U2tn:*w2ici3/c; 

eno; 

Ki:*UlC13*DCl 3-MJ1 C23*DC23+U1C33*DC33; 

K2 ; *U1 C 1 3*U2  C 13+U1C23*U2C23+U1 C33*U2C33 ; 

K3>U2C  1  3*DC  1  ]*U2[23*DC23*U2C33*DC33  i 
AC:«<KI-K2*K3)/U ,-K2*K2) ; 

BC:*(K2*Kl-K3)/( 1 ,-K2*K2> ; 
xi;-ac*uui3*dx*u9;  yi:*ac*uic23+dy*ub; 
2i:«ac*uu33-f; 

X2: *BC*U2C 13+C21FC13; 

Y2 : *BC*U2C23+C21FC23 ! 

22 : ■BC*U2C33*C2 1FC33 ! 

xt:»(xi+x2)/2. ;  yt:«(yi*yzi/z.;  zt:*<zi*zz)/z.: 


END 


VID.PAS 


/•••*••••*••••••••••••••••••••••••••••••••••<•«••••«••••/ 

/*  file:  vid.pas  */ 
/*  programmer:  sheng-jen  tsai  •  / 
/•  date:  march  12.  isss  */ 
/*  */ 
/*  function:  to  test  the  normal  operation  of  the  vision*/ 

/*  SYSTEM.  WHEN  THIS  PROGRAM  IS  EXECUTED.  */ 
/*  THE  11/70  COMPUTER  HILL  CONTROL  THE  «/ 
/*  SCANNING  OF  THE  VISION  SYSTEM  BY  SENDING  */ 
/*  ONE  BYTE  OF  DATA  TO  TRIGGER  THE  IMAGE  */ 
/*  DETECTION  EVENT.  THE  RESULTING  5  BYTES  */ 
/*  OF  DATA  ARE  SENT  BACK  TO  11/70  FOR  */ 
/*  TR I ANGULATION.  IF  NO  IMAGE  IS  SENSED  OR  •/ 
/*  THE  DETECTED  IMAGE  IS  OUT  OF  REACH  FOR  */ 
/•  THE  HEXAPOD.  MESSAGE  HILL  BE  DISPLAYED  ON  •/ 
/*  THE  TERMINAL  SCREEN  TO  PROMPT  FOR  ANOTHER  */ 
/*  TRY.  */ 
/♦  */ 
/*  PROCEDURES  CALLED!  TRION  */ 
/•  FUNCTIONS  CALLED:  ATAN2 . SIGN, ASIN  */ 
/*  */ 
/•  USER  GUIDE:  TKB  tVID.CMD  */ 
/*  ♦/ 
/••••••••••••••••••••«•••••••••••••**••»••••••••••••••••/ 


LABEL  10.20; 

CONST  CST-377B; 

HEIGHTT*  '  '  S 
HEIGHTS* '  ‘  I 
RESETSCREEN* ' ' ! 

PX1T* ' 
rxlb*  '  '5 
*»it»' 
rrlb*": 
nit*'  •; 
pzib* '  •; 
rxZt* ' ' ; 

*»2b« '  •: 
r  y  2 1  *  ' .'  I 
r  y2b*  '  '  5 
PNOt*  '  ”, 

Pnob* ' ' S 
POUTT*  '  ”, 

POUTB*  '  *; 

Hid** '  •: 
invvid*o* ' ' 5 
rtauidto*  ’  ”, 

BELL*"? 
clear* ' ' • 
p«l*ar« '  '1 

VAR  READY ,  IMAGE  .  N .  I .  J  .  A , B ,  SB  ,  X 1  ,  Y 1  .  X2 .  Y2 !  INTEGER 

XE .  YE .  ZE .  ANG ,  AND  1  ,  ANG2 ,  ANG3 :  REAL  ', 

T.T1B.TB1 ,TB2.TlZ:HATRIx: 

(••C 

•MCALL  ALUNCS . GIOHCS , GIOCS . ASTXCS . HTSECS . EXITCS 
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t 


i 


.EVEN 

stad: 

.BYTE 

TC.TBF.l 

rdbuf: 

.BLKB 

6 

iost: 

.  BLKW 

2 

wrbufc: 

.BYTE 

*800000010 

MRBUF 1 

.BYTE 

“BOOOOOOOl 

.EVEN 

*) 

PROCEDURE  TRION;  EXTERNAL; 


PROCEDURE  CHECK;  <  TO  CHECK  IF  THE  IMAGE  IS  OUT 
OF  REACH  FOR  THE  HEXAPOD  > 

CONST  L1-12.36I  L2-17.00)  L3*0. ; 

L4-  2.S;  L3*2 . 463 ' 

XHIP-22.73)  YHXP*8.3G2; 


VAR  X.Y.Z.A.B.C.D.E.SAI.THl .TH2.S:REAL) 
FUNCTION  ATAN2(Y.X  I  REAL)  I  REAL ;  EXTERNAL ; 
FUNCTION  AS IN I  VALUE  I  REAL)  I  REAL  I  EXTERNAL 
FUNCTION  S1GNCX  I  REAL)  I REAL ;  EXTERNAL) 
BEGIN 

x:-xe;  z:*ze-io.s; 

IF  YE  <  0.  THEN  YI *ABS < YE ) 

else  y:«ye; 
y:»y-yhip; 


<  COMPUTE  THE  ROTATIONAL  ANGLE  ANG1  > 

SA 1 1 ■ ATAN2 ( X » Y  > ; 

s:-sin(sad;  c:»cos(sai> ; 

SAI : »SAI » 180. /3 . 1 4 159; 

ANG1 I *ABS ( SA I  ) ; 

<  COMPUTE  THE  KNEE  JOINT  ANGLE  ANG2  > 

IF  S«0.  THEN  A:«0. 

ELSE  a:«(X-L4*S*L3*C>/S; 
a:*a*a: 

B:»A*2*Z-LI*L1-L2*L2-L3*L3; 

d:«li*li-»l5*l5; 

d:-sgrt<d) ; 

d:«2.*l2*d; 

b:«b/d; 

IF  ABS(B>  >  I.  THEN  B.'«SIGN(B> 

else; 

b: »asin<  b > ; 
e: >atan2 ( ls  >  l  i  > ; 
thz:«b-e; 

s:«sin(th2>;  c:«cos(TH2); 

TH2:-TH2*180./3. 14159; 

ANG2 : ■ ABS ( TH2 ) ) 

<  COMPUTE  THE  ELEVATION  ANGLE  ANG3  > 

a: *ls+l2*c ;  b:«li+l2*s: 
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c:*a*a+b*b: 
c:-sqrt<c> : 
c:«z/c; 

IF  ABS ( C )  >  1.  THEN  C:-SIGN(C> 

else; 

c:--asin<c>  : 

D : *ATAN2 ( A . B )  7 
TH1 I -C+D ; 

THi:-THl*180./3. 141591 
ANG3: *ABS(TH1 ) ; 
end;  <  OF  CHECK  > 

BEGIN 

TBici.n:«o.o995;  tbici ,23:*-o.9355;  tbici»3j:«o.34i; 
TBiti.4i:«-o.ou; 

TBlt2.n:*0.995;  TBl£2,23-.-0.0842;  TB1 12 • 33 : *-0.0341 ; 
TB1C2.43 :«0. 14; 

TBIC3,  13  :*0.  ;  TB1C3.23:*0.343T  TB1C3.33:«0.9392; 
TBlC3.43;«-0.4fl; 

TBic4.n:>o.:  tbic4.2i:>o.  :  tbic4,33:«o. :  tbu4.43:*i.; 
TB2C1  . 1  3  :  *-0.0995‘.  TB2t 1 . 2 3 1 =-0.9355;  TB21 1 . 33  :  -0.341 ; 
TB2ci.43:«-o.on; 

TB2C2 • 1 1 : *0 . 995 ;  TB2C2 > 23  I *-0 . 0842;  TB2C2 . 33 : -0.0341 ; 
TB2C2 . 4  3 : *-0.163; 

TB2C3. 1 3 : >0. ;  TB213>23:-0.343;  TB2C3. 33 : -0.9392; 
TB2C3,43:--0.4B; 

TB2t4.13:«0.;  TB2C4, 231-0.;  TB2C4.33 : -0. ;  TB2C4 . 43  I » 1 . ; 
FOR  I ; « 1  TO  3  DO 
BEGIN 

TiBCi.ni-TBici.n;  tibc2.i3:*tbici.23: 
TlBt3'I3:-TBlCI.33; 

TlBCI»43:*-<TBlCl>43*TBltl»I3+TBlt2«43*TBlC2.I3 

+TB1I3.43*TB1C3.I3>; 

end: 

T1BC4.13:«0.;  T1B£4,23:*0. ;  T1BC4,33:»0. ;  T1BC4.43 ! « 

FOR  i:«l  TO  3  DO 
BEGIN 

FOR  j:«  1  TO  4  DO 
BEGIN 

T12CI. J3:»TIBII. 13*TB2tl . J3+T1BC I , 23*TB2C2 . J3 
■♦TIBC  1 . 33*TB2C  3 .  J3  +  T1BCI » 43*TB2t  4  >  J  3 
end;  <  OF  J  > 
end;  <  of  i  > 
thatrix; »Ti2; 

writeln(resetscreen) ; 

HRITELN<  PY1 T .  INW1  DEO r  HIDE .  HEIGHTB  r-HEIGHTT » 

'  SPECIFY  FOOTHOLD  '>: 

HRITELN< PYlBr  INWIDEO, HIDE. HEIGHTT, HEIGHTB. 

'  SPECIFY  FOOTHOLD  ’)! 

MRITELN  (BELL ) ;  HR  I TELN  ( BELL )  *  MR  I TELN  <  BELL ) 

HRITELN( REGVIDEO ) ! 

io:  a:*o; 

<**C  EFl-7 

LUN-'DS 

ALUNtS  #LUNf  #"0T >  40 
QI0M4S  #10. ATT  >  #LUN 

0 1 OH*S  #BF . GMC . #L UN , #EF 1 . . , , < #ST AD ,  #2  > 


HOV  STAD. A(R5) 
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SMAB  A ( R5 )  •) 

a: -a  and  cst: 

IF  A  >  0  THEN 

BEGIN 

UR I TELN (  '  UNSOLICITED  DATA  AI3, 

'  BYTES  EXISTS  • 

<**C  EXIT4S  *) 

END 

else; 

<**C 

QIOUSS  #IO.UAL.#LUN,#EFl , .#IOST, ,<#URBUF,#1> 

QI0M4S  #10 . RAL ! TF .  RNE . #LUN . #EF1 , . # I OST .  . <  #RDBUF . #3  > 
MOVB  RDBUF  >  SB ( R5  > 

MOVB  RDBUF+1 ,X1 <R5> 

MOVB  RDBUF+2. Y1 <R5) 

MOVB  RDBUF+3  >  X2 ( R5 ) 

MOWS  RDBUF+4 , Y2<  R5 )  *) 

xpi:*xi  and  cst; 
ypi:«yi  and  cst; 

XP2I-X2  AND  cst; 

YP2I-Y2  AND  CST; 

SB: -SB  AND  cst; 
a: -SB  AND  s; 

IF  A  -  0  THEN 

GOTO  20 

else; 

ready: -sb  and  i; 

IF  READY-0  THEN  GOTO  10 

else; 

a: -sb  and  4;  b:»sb  and  is; 

IF  < A-0)  OR  <B-0)  THEN 
BEGIN 

UR I TELN ( RESETSCREEN ) ! 

WR I TELN <  PnoT . INVV IDEO . WIDE . HEI GHTB  . HE I GHTT , 

'  NO  IMAGE.  TRY  AGAIN  » '); 

WRI TELN ( PnoB  > INVV I DEO. HIDE .HE I GHTT . HE I GHTB. 

'  NO  IMAGE.  TRY  AGAIN  !'>; 

HR I TELN (BELL)! 

<**C 

QlOUtS  flO.HAL >#LUN. #EF1 , .*IOST, .<#WRBUFC. #1 > 

«) 

GOTO  10 ; 

END 

ELSE 

BEGIN 

IRION; 

XE : -XT*TB1 I 1 . 1 3*YT*TB1 C 1 . 23*ZT*TB1 C 1 .33+TBl C 1 . 43 1 
YE!  -XT*TB1  C2.13,*,YT*TB1  CZ . Z3+ZT*TB1  C2.33+TB1C2.43; 
ZE:-XT*TB1C3. 13«-YT*TBIC3.23*ZT*TB1£3.33+TB1C3.43; 
xe:-xe/o.o254;  ye:-ye/o.o254;  ze:«ze/o.o2m; 

HRITELN(RESETSCREEN) ; 

URITELN<  '  X1-'.XP1.'  Y1-'.YP1.'  X2«'.XP2.'  Y2-'.YP2) 

writtln(-xlt.inuuidtL.uid»,h«i«htb . haimhtt . 
t  *  9  xE  :b,'4  r  '  I NCH  ' )  } 

HRITELN( PX1B • INVVIDEO . UIDE  >  HE I GHTT . HE I GHTB. 

'  x-'.xe:b:4.'  inch  - > ; 
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WRITELN(PY1T, INWIDEO,  WIDE,  HEIGHTB  ,  HEIGHTT  , 

•  y-'.ye:8:4,'  inch  •>; 

WRITELN<  PY1B , INWIDEO ,  WIDE . HEICHTT , HEIGHTB , 

•  Y"'.ye:b:4.'  inch  •>: 

HRITELN ( PzlT,  INWIDEO .  WIDE .  HEIGHTB  ,  HEIGHTT . 

'  z-'.ze:b:4.'  inch  •>; 

URITELNtPilB.  INWIDEO. WIDE. HEIGHTT, HEIGHTB. 

'  z* '  >  ze :s : 4  > '  inch  '>; 
check : 

IF  ( ANG1  >  75.)  OR  ( ANG2  >  75.)  OR  (ANG3  >  75.)  THEN 
BEGIN 

MRITELN ( BELL  >  J  WRITELNf BELL ) I 
WRITELN(POUTT,  INWIDEO, HIDE.  HEIGHTB. HEIGHTT, 

'  OUT  OF  REACH,  TRY  AGAIN  !  ')! 

WRITELNt  POUTB .  INWIDEO,  WIDE  >  HEIGHTT  ,  HEIGHTB  , 

'  OUT  OF  REACH,  TRY  AGAIN  ! 

END 

else; 

<**C 

GI0W4S  PIO.WAL. #LUN. #T ‘ 1 ,  # I OST , , < PWRBUFC , # 1 > 

«) 

goto  to; 
end; 

hr iteln<reg video) ; 

writ* In <  '  ' )  I 


CAC1 . PAS 


/*  file:  CAC1.PAS  */ 
/*  programmer:  sheng-jen  tsai  */ 
/*  date:  march  12.  tss3  •/ 
/*  */ 
/*  function:  to  read  the  s-tuple  <RX.RY.RZ. IX. IY)  FROM  AN  */ 
/*  EXTERNAL  FILE  CAC1.DAT,  COMPUTE  THE  DISTORTED*/ 
/*  IMAGE  RADIUS  IR.  THE  UNDISTORTED  RADIUS  RR.  */ 
/*  AND  THE  AMOUNT  OF  DISTORTION  DR.  THE  VALUS  */ 
/*  OF  RR  AND  DR  ARE  STORED  IN  A  SECOND  EXTERNAL  */ 
/*  CLEN1.DAT  WHICH  IS  TO  BE  USED  BY  THE  CURVE  */ 
/*  FITTING  ROUTINE  PLYFT1.PAS.  */ 
/*  */ 
/•  PROCEDURES  CALLED!  ROOT  (  IN  FILE  ROOT 4 . PAS  )  •/ 
/•  FUNCTION  CALLED!  ATAN  (  IN  FILE  ATAN.PAS)  •/ 


/•  USER  GUIDE!  CAC 1 /FP/CP»CAC1 >  ROOT  4 .ATAN . 1 1 . 1 3PASLIB/LB 

/* 


PROGRAM  CACi: 


F*6.5E-35 

DXM.SE-Sf  DY»3.BE-3: 
NX-248:  NY-244; 


OBJECT = 

RECORD 
ixiinteger; 
iy: integer; 
rxireal; 
ry:real 
end: 

BLOCK -FILE  OF  OBJECT: 

ITEM- 

RECORD 

rr:real; 

DR  .’REAL 

end; 

GROUP-FILE  OF  ITEM; 

PROCEDURE  ROOT <K4.K3.K2.Kl . KO : REAL : 

var  r. ir:real > ;  external: 

FUNCTION  AT  AN ( Y . X : REAL  > : REAL ;  EXTERNAL; 


CR.M.RR.X.Y.A.Z.B. IR.DR.XUD.YUD.C. 
RX.RY.K4.K3.K2.K1.K0.DIF, ANG1 .ANG2:REAL: 

SIZE. IX. I Y, IXC. IYC. ixud. iyud: integer: 

bufferiobject; 

cackblock; 

cell: item; 

cleni:group; 


begin 
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NRITELN(  '  INPUT  2  DISTANCE  '>; 

READLN(Z) t 
si2E.*»i; 

REWR I TE  <  CLEN 1 .  'CLEN1.DAT', 'CLEN1 . DAT  SIZE ) 
RESET (CACl . 'CAC1.DAT ' , 'CAC1 .DAT ' .SIZE) ! 
WRITELNI 'RX  RY  IX 

'  I Y  ' •  '  IXUD  IYUD 

'  ixc  iyc  •>; 

/***  READ  RX.RY.IX.IY  FROM  FILE  CAC1.DAT  ***/ 

WHILE  NOT  EOF (CACl )  DO 
BEGIN 

buffer:»cacia: 

ix: “BUFFER . ix: 
iy:*buffer. iy: 
rx: -SUFFER. RX! 

ry:=buffer.ry; 

RX I *RX*2 . 54  J 
GET (CACl ) S 

/***  COMPUTE  THE  DISTORTED  IMAGE  RADIUS  IR  ***/ 

a:*«ix-u9)*dx; 
b:*< iy-jib)*dy: 
c  r  *  a  * 

angi :«atan(b.a> : 
ir:«sqrt(a#a+b*B) : 


/***  COMPUTE  THE  UNDISTORTED  IMAGE  RADIUS  RR  ***/ 

K4I  »-B . 420006E5  )  K3: =7 . B77979E3 : 

K2I *1 .69781SE1 ) 

Kt :*-I . 11459  IE- 1  ;  KO:*l . 1785B0E-4) 

ROOT (K4.K3.K2.K1 .KO.CR. IR) ! 

m:»z/f: 

x:*(rx*o.oi*dx*i ig*M)/(M+i > ; 
y:«<ry*o.oi+dy*ub*m)/(m+i  ) ; 
xud:*x/dx; 
yud:*y/dy: 
ixud:*round(xud> : 

IYUD I “ROUND ( YUD ) I 

x:«x-dx*u9t 

y:*y-dy*ub; 

ANG2I »ATAN( Y.X)  ) 

ang2:*anG2*ibo./3.14is: 
rr:»sqrt<x*x*y*y) : 

A : «CR*COS< ANGI )/DX) 

b:«cr*sin(angd/dy; 

IXC : » 1 18+ROUND  <  A ) ; 

IYC: *1 18+R0UND(B)  : 

IF  C  <  0.  THEN  IYC:»IYC*2 

else; 

dr:«rr-ir; 
dr:»abs(Dp> ; 
dif:«rr-cr; 

/**«  STORE  RR  AND  DR  INTO  FILE  CLEN1.DAT  ***/ 

cell.rr:*rr; 
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f 


cell.dr:*dr: 

cleni“:«cell: 

PUT (CLEN1 > ; 
wr;teln:writeln; 

WRITELN<RX:5 12  »  '  '.RY:5:2.'  '.IX:5 

iy:s.'  ',ixud:s.'  '.iyud:s. 
ixc:s.'  ',iyc:s>: 

end: 

END. 


IM20B.PAS 


PROGRAM  IM20B * 

/***************************************#**•**#****#***********#***/ 
/*  programmer:  s.  j.  tsai  •/ 

/*  DATE  :  MAY  5  1982  */ 

/*  */ 

/*  THIS  PROGRAM  IS  THE  "INVERSE  PROGRAM"  OF  OS2IM,  IT  TAKES  */ 

/*  THE  IMAGE  COORDINATE  (IX.IY)  AND  COMPUTE  THE  THREE  DIMENSIONAL  */ 
/*  SPATIAL  COORDINATE  <X,Y,2>  OF  THE  OBJECT  POINT . ASSUMING  THAT  */ 
/*  THE  OBJECT  POINT  LIES  ON  A  PLANE  WHICH  IS  PERPENDICULAR  TO  THE  */ 
/*  OPTICAL  AXIS  OF  THE  CAMERA  AND  THAT  THE  DISTANCE  BETWEEN  THE  */ 
/*  LEN  CNETER  AND  THE  OBJECT  PLANE »  2  ,  IS  KNOWN.  */ 

/*  */ 

/••••••••••••••••••••••••••••••••••••••••it*************************/ 


CONST 

F«6.5E-3; 

DY-3.6E-5: 

DX*4.GE-5; 

VAR 

LX.LY.X.Y.Z.MIREAL! 

ix, iy. i : integer; 

BEGIN 

lx:*24b.*dx: 

ly:*244.*dy; 

2: * i . ; 

WHILE  2  <*  2.  DO 
BEGIN 

PAGE (OUTPUT) ; 

WRITELN<  '  2  IX  X 

'  IY  '  ,  '  Y  '  )  ; 

m:»z/f; 

ix:»2: 

iy:«2; 

FOR  i:»l  TO  120  DO 
BEGIN 

x:«dx*ix+(dx*ix-lx/2. >*m; 
y:«dy*iy'MDy*iy-ly/2.  >*m; 
writelncz: io:3, ix:s,x: 10:3, iy:s- y: io:d) ; 
ix:«ix*z; 
iy:«iy*z; 

end;  /*  end  iy  •/ 

z:-z*o.i ; 

end;  /*  END  2  */ 

END.  /*  END  IMZOB  •/ 
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0B2IM.PAS 


PROGRAM  OBZIM) 

/** . ***********♦************•**********#********•♦********♦***/ 

/*  programmer:  s.  j.  tsai  */ 

/•  DATE  :  MAY  5  1382  »/ 

/*  */ 

/»  THIS  PROGRAM  RELATES  A  THREE  DIMENSIONAL  POINT  WHOSE  */ 

/«  COORDINATE  IS  (X.Y.Z)  TO  ITS  IMAGE  , (IX.IY).ON  THE  FOCAL  PLANE  */ 
/*  THIS  INFORMATION  IS  USEFUL  TO  CAMERA  CALIBRATION . GIVEN  ANY  #/ 

/*  POINT  (X.Y.Z)  ON  A  PLANE  WHICH  IS  PERPENDICULAR  TO  THE  OPTICAL  */ 
/*  AXIS  OF  THE  CAMERA. THE  CORRESPONDING  IMAGE  COORDINATE  (IX.IY)  */ 
/*  IS  COMPUTED. NOTE  THAT  IX  DEPENDS  ONLY  ON  X  AND  Z. WHILE  IY  */ 

/«  DEPENDS  ONLY  ON  Y  AND  Z.  */ 

/*  */ 

/«  parameters:  */ 

/*  F  I  FOCAL  LENGTH  OF  THE  LENS  USED.  */ 

/*  DX  I  THE  WIDTH  OF  A  PIXEL,  */ 

/*  DY  I  THE  HEIGHT  OF  A  PIXEL.  */ 

/•  NX  :  NUMBER  OF  PIXELS  IN  A  ROW,  •/ 

/*  NY  :  NUMBER  OF  ROWS  IN  A  FRAME.  •/ 

/*  */ 


CONST 

F-6.5E-3: 

DY-3.BE-5; 

DX-4.BE-3: 

NX>248; 

NY«244  J 

VAR 

px,py.lx.ly.x.y.z.m:real: 
ix. iy:integer: 

BEGIN 

lx:*nx«ox: 

ly:»ny*dy; 

2  "  *  1  •  r 

WHILE  Z  <»  2.  DO 
BEGIN 

PAGE (OUTPUT) ! 

WRITELN(  '  Z  X  IX 

'  Y  IY  ■)'. 

h:*z/f; 

y:*-i.j 

x:«-i.: 

WHILE  Y  <■  l.  DO 
BEGIN 

PX:»<  <X+LX*M/2.  >/(M«-l )  >/dx; 

ix:-round(px)  ; 

PY:-(  <Y*LY*M/2.  )/<M4-I )  )/DYJ 

iy:»round<py)  ; 

writeln(z: 10: 3,x : 10:3. ix:s.y: 10:3, iy:s> ; 

x:«x*o.ot; 

y:»y«-o.oi; 

end;  /*  END  y  «/ 

z:-z*o.i: 

end;  /•  END  z  */ 

END.  /*  END  082 I M  •/ 
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END 

DATE 
FILMED 

—  83 

DTIC 


V 


